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ABSTRACT 

A  pure  pursuit  guidance  law  is  combined  with  a  heading 
autopilot  to  provide  accurate  path  keeping  of  submersible 
vehicles.  The  scheme  is  implemented  and  analyzed  in  both  the 
horizontal  and  vertical  planes.  A  complete  stability  analysis 
is  performed  in  order  to  evaluate  regions  of  stable  vehicle 
operations.  Numerical  integrations  support  the  analytic 
predictions.  Two  distinct  stability  boundaries  are 
established.  In  the  first,  the  vehicle  loss  of  stability  is 
accompanied  by  the  generation  of  oscillatory  motions  around 
the  commanded  path.  In  the  second,  loss  of  stability  occurs 
with  linearly  increasing  path  deviation.  The  horizontal  and 
vertical  plane  schemes  are  combined  with  a  propulsion  control 
law  in  order  to  achieve  path  tracking  of  a  general  commanded 
route  composed  of  several  straight  line  segments  in  three 
dimensional  space. 


111 


C. 


TABLE  OF  CONTENTS 

I  INTRODUCTION   1 

II.  HORIZONTAL  PLANE   5 

A.  EQUATIONS  OF  MOTION  5 

B.  CONTROL  LAW 6 

1.  ZERO  YAW  ANGLE 7 

2  .  NON  ZERO  YAW  ANGLE 9 

C.  GUIDANCE 11 

D.  STABILITY 12 

E.  SIMULATIONS 17 

III.  VERTICAL  PLANE 19 

A.  EQUATIONS  OF  MOTION 19 

B.  CONTROL  LAW 2  0 

1.  ZERO  PITCH  ANGLE 22 

2.  NON  ZERO  PITCH  ANGLE 27 

C.  GUIDANCE  LAW 30 

1.  HORIZONTAL  PATH 30 

2.  INCLINED  PATH 31 

D.  STABILITY 31 

1.  REGIONS  OF  STABILITY 33 

2.  SIMULATIONS 35 


IV 


i 

1.  REGIONS    OF    STABILITY 33 

2.  SIMULATIONS 35 

E.  ANALYSIS 40 

F.  STEADY    STATE    SOLUTIONS       48 

IV    THREE    DIMENSIONAL    GUIDANCE    CONTROL    56 

A.  PROPULSION    CONTROL       5  6 

B.  THREE    DIMENSIONAL    PATH    KEEPING       59 

CONCLUSIONS    AND    RECOMMENDATIONS     71 

APPENDIX    A 73 

APPENDIX    B 90 

APPENDIX    C 96 

LIST   OF    REFERENCES 101 

INITIAL    DISTRIBUTION    LIST    103 


v 


LIST  OF  FIGURES 

Figure  1.   Horizontal  plane  geometry   10 

Figure  2.   Regions  of  stability  in  the  horizontal  plane  16 

Figure  3.   Stable  and  unstable  numerical  simulations   .  18 
Figure  4.   Vertical  plane  geometry:  Horizontal  commanded 

path 2  4 

Figure  5.   Vertical  plane  geometry:  Inclined  commanded 

path 2  8 

Figure  6.   Regions  of  stability  for  u=5  ft/sec  and  zGB=0 . 1 

ft 34 

Figure  7.   Numerical  simulations  in  region  2   36 

Figure  8.   Numerical  simulations  in  region  1   37 

Figure  9.   Numerical  simulation  in  region  3  38 

Figure  10.   Regions  of  stability  for  zGB=0  and  for  any 

speed  u 39 

Figure  11.   Regions  of  stability  for  zGB=0.1  ft     ...  45 

Figure  12.   Regions  of  stability  for  u  =  2  ft/sec    .  .  46 

Figure  13.   Critical  value  of  tv  versus  u  and  zGB   ...  47 

Figure  14.   Steady  state  pitch  angle  6  versus  zGB   ...  52 

Figure  15.   Steady  state  dive  plane  angle  6  versus  zGB  53 
Figure  16.   Steady  state  0  versus  zGB  for  several  values 

of  tv 54 

Figure  17.   Steady  state  6  versus  zGB  for  several  values 

of  u 55 


VI 


Figure   18.    Coordinate   transformation   for   3-D  path 

keeping 6  3 

Figure  19.  Horizontal  plane  rotation  64 

Figure  20.  Vertical  plane  rotation  65 

Figure  21.  Numerical  simulation  for  3-D  path  keeping  .  66 

Figure  22.  Time  history  of  vehicle  speed  u 67 

Figure  23.  Time  history  of  propeller  revolutions  per 

minute 68 

Figure  24.  Time  history  of  rudder  angle 69 

Figure  25.  Time  history  of  dive  plane  angle 70 


vn 


I  INTRODUCTION 

One  of  the  most  significant  functions  of  an  underwater 
vehicle  is  accurate  path  control  for  transiting  along 
prescribed  routes  in  three  dimensional  space.  The  commanded 
path  is  usually  described  by  a  series  of  way  points  in  space 
and  time  either  by  the  commander  or  by  a  path  planner  function 
in  the  case  of  an  unmanned  vehicle.  Without  significant  loss 
of  generality  we  can  assume  that  the  commanded  path  can  be 
approximated  by  straight  line  segments  between  consecutive  way 
points.  This  assumption  does  not  alter  the  important  features 
of  the  path  keeping  problem  since  every  smooth  path  can  be 
approximated  arbitrarily  closely  by  a  series  of  straight  line 
segments.  Once  a  desired  straight  line  path  has  been 
generated,  the  vehicle  guidance  and  autopilot  functions  are 
called  upon  to  ensure  satisfactory  path  keeping  through  the 
use  of  the  vehicle  actuators. 

One  way  to  ensure  that  the  vehicle  goes  through  a 
specified  sequence  of  way  points  is  by  using  a  heading 
autopilot  coupled  with  a  line  of  sight  guidance  scheme  [1]. 
The  scheme  proved  to  be  robust  enough  so  that  when  coupled 
with  an  independently  developed  depth  autopilot  [2],  accurate 
depth  control  was  maintained  while  transiting  between  way 
points  in  the  horizontal  plane.  The  disadvantage  associated 
with  this  technique  is  that  the  actual  vehicle  path  between 


two  consecutive  way  points  differ  significantly  from  the 
corresponding  straight  line  segment. 

In  order  to  overcome  this  problem  and  achieve  accurate 
path  control  in  the  presence  of  obstacles  and  underwater 
currents,  a  cross  track,  error  autopilot  was  developed  for  the 
horizontal  [3]  as  well  as  the  combined  horizontal  and  vertical 
planes  [4].  A  cross  track  error  autopilot  incorporates  the 
deviation  of  the  assumed  straight  line  path  into  the  control 
law  design.  This  requires  the  introduction  of  additional 
kinematic  relations  in  the  control  design  and,  as  a  result, 
the  controller  tends  to  be  more  sensitive  to  actual  system  / 
mathematical  model  mismatch. 

The  main  drawback  of  a  cross  track  error  autopilot  is  that 
it  represents  a  combined  guidance  /  control  scheme  with  no 
clear  distinction  between  these  two  functions.  Thus  it  is  very 
vehicle  specific  and  offers  little  flexibility  in  the  design. 
Path  control  is  limited  to  cross  track  error  only  and  analysis 
of  alternate  schemes  [5]  is  not  possible  unless  the  combined 
scheme  is  redesigned.  For  this  reason  we  decide  to  separate 
once  more  the  guidance  and  autopilot  functions  of  the  vehicle. 
An  orientation  controller  is  designed  in  order  to  provide 
accurate  vehicle  headings  in  response  to  guidance  commands. 
The  controller  is,  thus,  based  on  the  vehicle  dynamical 
equations  and  Euler  angle  rates.  A  guidance  scheme  is  used  to 
provide  appropriate  heading  commands  through  the  kinematic 
equations  of  inertial  position  rates.  A  line  of  sight  guidance 


command  law  is  employed  as  in  [6]  and  [7].  We  consider  a 
reference  point  that  is  moving  ahead  of  the  vehicle  at  a 
constant  distance  on  the  desired  straight  line  path.  We  refer 
to  this  distance  as  the  lookahead  distance.  The  commanded 
heading  is  then  equal  to  the  line  of  sight  angle  between  the 
center  of  the  vehicle  and  the  lookahead  point.  By  suitably 
selecting  the  lookahead  distance  the  degree  of  convergence  of 
the  guidance  law  can  be  varied  from  very  slow  to  very  rapid 
onto  the  straight  line  path. 

Although  the  above  scheme  appears  to  be  trouble  free  on 
the  surface,  a  significant  complication  arises  in  the  case  of 
underwater  vehicles.  Since  the  actual  vehicle  response  is 
relatively  slow  as  dominated  by  the  existence  of  important 
dynamical  lags  there  is  the  possibility  of  instability  when 
the  guidance  and  control  functions  are  combined.  High  values 
of  the  lookahead  distance  result  in  very  slow  vehicle 
response.  The  problem  is  then  to  evaluate  these  regions  of 
stable  and  unstable  vehicle  response.  Chapter  II  of  this 
thesis  summarizes  the  stability  analysis  results  for  the 
horizontal  plane.  In  Chapter  III  we  proceed  with  the  analysis 
of  motions  under  the  guidance  and  control  scheme  for  the 
vertical  plane.  It  is  shown  that  the  existence  of  hydrostatic 
restoring  moments  here  due  to  the  nonzero  (positive) 
metacentric  height  brings  in  an  additional  form  of  instability 
not  present  in  the  horizontal  plane.  Finally  in  Chapter  IV  the 
previous  two  guidance  and  control  schemes  for  the  horizontal 


and  vertical  planes  are  combined  and  with  a  speed  autopilot, 
accurate  path  tracking  in  three  dimensional  space  is  achieved. 
The  main  conclusion  of  this  work  is  that  guidance  and  control 
laws  for  underwater  vehicles  must  be  designed  together  even  if 
they  are  kept  separated,  in  order  to  ensure  stable  and 
satisfactory  path  keeping.  All  computations  in  this  work  are 
performed  for  the  Swimmer  Delivery  Vehicle  [8]  for  which  a 
complete  set  of  hydrodynamic  coefficients  and  geometric 
properties  is  available. 


II.  HORIZONTAL  PLANE 

In  this  section  the  vehicle  equations  of  motion  for  the 
horizontal  plane  (x,y),  the  design  of  a  heading  autopilot  and 
simulations  and  stability  results  are  presented. 

A.   EQUATIONS  OF  MOTION 

For  the  horizontal  plane  the  mathematical  model  consists 
of  the  nonlinear  sway  and  yaw  differential  equations  shown 
below: 

m(v+uT+xGf-yGT2)  =Y  C2-1) 

I zr+mxG(v+ur) -myGur=N  (2.2) 

Equations  (2.1)  ,(2.2)  can  be  easily  derived  from  the  general 
six  degrees  of  freedom  equations  for  a  vehicle  by  assuming  all 
terms  off  the  horizontal  plane  to  be  zero.  The  equations  for 
the  sway   force  Y  and  yaw  moment  N  are  presented  below: 

Y=Y,t+(Y^+Yzur)+Yvuv-£-[[cDh(Z)   {^+\r\3]dZ,  +  Ybu2b 
1  vi  v         2  J        y  \v+$r\ 

N=Nit+(N^+Nrur)  +Nvuv--2-[[CDh(Z)    ^+\Z\*  V  dl+Ntu26 

i  vi  v         2  j       uy  y+tr 


To  complete  the  model,  expressions  of  the  inertial 
position  rates  and  yaw  rate  are  required.  These  are  the 
kinematic  equations: 

$=r  (2.3) 

x=ucos\|r-vsini|/  (2.4) 

y=usini|r-vcosi|f  (2.5) 

B.       CONTROL   LAW 

It  is  more  convenient  for  the  design  of  a  linear  state 
space  heading  controller  to  represent  the  above  equations 
(2.1) , (2.2) , (2.3)  in  the  following  form  (with  yG=0): 

i|r=r  (2.6) 

v=alluv+a12ur+b1u2b+dv(  v,  r)  (2.7) 

r=a^1uv+a22ur+b2u2b+dr  ( v,  r)  (2.8) 


where 


D=(Iz-Ni)  (m-Yj)  -(mxG-Yt)  (mxG-Nv) 


a11  =  ^[(J2-J^)rv-(/nxG-Ff)^vr] 


«3i2  =  ^[(J,-^r)  (m-Yz)  -  (mxG-Yz)  (-mxG+Nz] 


a2x-\  I  (m-Y*)Nv-(mxG-Nj  Yv] 


a2i  =  \  i(m-Y^)  (-mxG+Nr)  -  (mxa-N+)  (-m+Yz 


b^+Ul.-NjYi-lmxg-YjNj 


b2  =  ±[(m-Y.)Yb-(mxG-NjY6} 


dv(v,  r)  =-±±pCDy[  (Iz-Nt)  1,  +  Y;!,) 


dz(v,r)=-±±pCDy[(m-Yv)I1+N*l2] 


Ix=f[h(Z)  (v+Zr)  \(v+lz)  \]di 

I2=f[h(Z)  (v+Zz)  |(v+£r)  \l]dl 

The  nonlinear  terras  d  ( v, r ) ,d  (u,  r )  are  small  and  can  be 
neglected  for  control  law  design.  They  are  kept,  however,  in 
all  numerical  simulations  that  follow. 

1.   ZERO  YAW  ANGLE 

When  the  commanded  yaw  angle  of  the  vehicle  is  zero  the 
control  law  has  the  following  form: 


b=k1ty+k2v+k3r  (2.9) 

where  k^k^kj  are  computed  so  the  system  will  have  the  desired 

dynamics.  The  closed  loop  characteristic  equation  has  the 
following  form: 

A.3+a1A2+a2A+a3  =  0  (2.10) 

where: 

a1=a11u+a22u+b1u2k2+b2u2k2 

a3  =  (bza^-bxa2X)  u2kx 

The  characteristic  equation  is  specified  in  the  following 
way.  It  can  be  chosen  to  satisfy  the  minimum  ITAE  criterion 
where  it  assumes  the  form: 

X3+a,A.2+a-A+a,=0  (2.11) 

1       2'     3 


where : 


ox  =  l  .75co0 


a2=2  .  15o>o 


a3=o)o 


,,  _  10U 


and  tH  represents  the  dimensionless  settling  time  for  the 
system.  Equating  the  coefficients  of  equation  (2.10)  with  the 
desired  equation  (2.11)  and  after  some  algebra  we  find: 

kx  = 3- (2.12) 

k2{bxa22-b2aX2)  u^k.ib^^-b^^)  u3=a2+b2u2k±         (2.13) 
k2bxu2  +k2b2u2  =  -a  1-  (axx+aZ2)  u  (2  .  14) 

Selecting  a  value  for  tH  according  to  the  ITAE  criterion, 
dictates  complex  conjugate  dominant  poles  with  oscillatory 
transient  response.  It  was  found  that  other  poles  selections 
(for  example  real  negative)  do  not  change  significantly  the 
nature  of  the  results  and  the  stability  boundaries  that  are 
presented  later. 

2.   NON  ZERO  YAW  ANGLE 

If  the  commanded  yaw  angle  is  non  zero  and  equal  to  i|f 
then  the  control  law  (2.9)  is  simply  modified  to: 

6=ic1(iJ;-\|;c)  +k2v+k3z  (2.15) 


Figure  1.   Horizontal  plane  geometry 
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No  feedforward  term  is  necessary  in  (2.15)  since  no  rudder 
angle  is  required  to  keep  the  vehicle  to  a  constant  non  zero 
heading  angle  at  steady  state. 

C.   GUIDANCE 

The  heading  autopilot  that  was  designed  in  the  previous 
section  is  called  upon  now  to  provide  vehicle  path  in  the 
sense  of  passing  through  a  series  of  way  points  in  the 
horizontal  plane.  In  order  to  achieve  it  without  changing  the 
previously  designed  heading  autopilot  we  have  to  couple  it 
with  a  suitable  navigation  scheme  such  as  line  of  sight 
guidance. 

The  simplest  such  guidance  law  is  a  pure  pursuit 
navigation  which  is  accomplished  as  follows.  The  autopilot 
attempts  to  point  the  longitudinal  axis  of  the  vehicle  towards 
a  point  D  which  is  located  ahead  to  the  vehicle  on  the  nominal 
straight  line  path  at  a  fixed  distance  xd  as  shown  in  Figure 
1.  This  target  distance  xd  to  as  the  visibility,  lookahead,  or 
preview  distance.  The  line  of  sight  angle  o    is  defined  by: 

tano  =  -■-£-  (2.16) 

Pure  pursuit  navigation  then  corresponds  to  taking: 

i|r0=o  (2.17) 
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as  the  commanded  heading  angle  in  the  control  law  (2.15). 

It  can  be  seen  now  that  the  commanded  vehicle  heading 
angle  is  not  constant  but  it  is  function  of  the  vehicle 
position  y.  This  introduces  the  lateral  deviation  eguation 
(2.5)  into  the  problem,  and  since  the  control  law  was  based  on 
eguations  (2.6), (2.7)  and  (2.8)  only,  stability  of  the 
combined  autopilot-guidance  scheme  is  no  longer  guaranteed. 
Therefore,  we  need  to  develop  conditions  which  will  guarantee 
stability   and  ensure  satisfactory  path  keeping. 

D.   STABILITY 

The  complete  system  is  given  by  the  differential  equations 
(2.6), (2.7), (2.8),  the  control  law  (2.15),  and  the  guidance 
equations  (  2  .  16 ) , ( 2 . 17 ) .  The  trivial  equilibrium  state 
corresponding  to  a  straight  line  motion  is  characterized  by: 

i|r  =  v=r=y=0 

Linearization  of  the  state  equations  gives  the  following 
linear  system: 

X=AX 

where   the   complete    state   vector    is: 

X=[i\i ,v,  r,y] 
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Local  stability  properties  are  established  by  the  eigenvalues 
of  [A]  The  characteristic  equation  is  found  to  be: 

AXA+Bk3+Ck2+Dk+E=0  (2.18) 


where 


A=l 


and 


B=-B1-C1 


C=-D,+B1C-aB7-A~ 


D=  -  CXD2  +DX  C2  -  UD2  -AXB2  +A2  B± 


A1=b1u2kl 


A2=b2u2kx 


B^a^u+biU2^ 


B2=a21u+b2u2k2 


C1=a12u+b1uzk2 


C2=a22u+b2u2k3 


2,-       1 


D1=b1uzk1 


xd 
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D7=b?u2k,— 


Loss  of  stability  occurs  when: 

BCD-B2E-AD2=0  (2.19) 

Equation  (2.19)  is  derived  from  Rooth ' s  criterion  for  (2.18), 
and  it  corresponds  to  a  pair  of  complex  conjugate  roots 
crossing  the  imaginary  axis.  After  some  algebra  equation 
(2.19)  is  simplified  to: 

.2 


a1x^+a2xd+ai=0  (2.20) 


where : 


1  1       Z  J 


a  _  (a1a2-2a3)  (bxa22-b2aX2-b2)  b^.a,  ^ 

2"  ^n-^i  (b2axl-bxa21)  u      l 


a3  = 


-  (^1^22 -^2^12-^2^  [£i<*i+  (b^^-b^^-b^  u]  a. 
(b2a11-b1a21)2u 


The    positive     root     of     equation     (2.20)     determines     the 
critical  value  of  xd   for   stability.    For   every  xd   >   xd  cpitical  the 
system  is   stable  which  means  that  the  vehicle  will   follow  the 
path.     In    the    opposite    case    where    xd     <     xd  criticaL    the    system 
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becomes  unstable  and  the  motion  of  the  vehicle  becomes 
oscillatory  as  a  result  of  a  complex  conjugate  pair  of 
eigenvalues  with  positive  real  parts. 

Results  for  the  dimensionless  critical  visibility  versus 
settling  time  tH  are  presented  in  Figure  2.  These  results  are 
independent  of  the  forward  speed  since  gains  k^,k2,k5  are 
functions  of  u.  It  can  be  seen  from  Figure  2  that  for  higher 
tH  (softer  controller)  higher  lookahead  distance  xd  is  reguired 
in  order  for  the  system  to  remain  stable.  It  is  obvious  that 
very  high  values  of  xd  correspond  to  a  very  slow  navigator 
with  a  loss  in  speed  of  response  and  navigational  accuracy. 
The  results  of  this  section  establish  analytically  the  minimum 
required  lookahead  distance  that  is  required  for  stability 
based  on  linear  approximations. 

It  should  be  mentioned  that  all  results  in  this  work  are 
presented  in  dimensionless  form  unless  otherwise  mentioned. 
Nondimensionalizations  are  performed  by  using  the  vehicle 
length  and  the  vehicle  forward  speed. 
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Figure  2.   Regions  of  stability  in  the  horizontal  plane 
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E.   SIMULATIONS 

Numerical  simulations  confirm  the  results  of  the  stability 
analysis  of  Figure  2.  The  simulated  lateral  distance  y  (in 
vehicle  lengths)  versus  time  t  (in  dimensionless  seconds)  is 
shown  in  Figure  3  for  two  cases.  The  nominal  straight  line 
path  is  y=0  Case  1  is  located  in  Region  1  of  Figure  2  and  it 
can  be  seen  that  the  vehicle  response  is  unstable.  Case  2 
corresponds  to  a  stable  (tH,xd)  combination  and  the  vehicle 
converges  to  the  desired  path. 
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1:  tH  =  4,  xd  =  0.3 
2:  t„  =  4,  xd  =  2 
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Figure  3.   Stable  and  unstable  numerical  simulations 
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III.  VERTICAL  PLANE 

In  this  section  the  vehicle  equations  of  motion  for  the 
vertical  plane  (x,z),  the  design  of  a  vertical  heading 
autopilot  and  simulations  and  stability  results  are  presented. 

A.   EQUATIONS  OF  MOTION 

Restricting  our  attention  to  the  vertical  plane  the 
mathematical  model  consists  of  the  nonlinear  heave  and  pitch 
differential  equations  shown  below: 

(3.1) 

m  (  w-  uq-xGq-  zGqz )  =Z 

Iyq-mxG  ( w-  uq)  +mzGwq=M  (3*2) 

where  only  vertical  plane  related  terms  have  been  kept.  The 
heave  force  Z  and  pitch  moment  M  are  written  as: 


Z=Zdq+{Z<,w+Zauq)  +Zwuw-^-  fcD  b{x)    (,^  xq\    dx+  (W-B)  cos6 
q  w        q  w         2  J      z  \w-xa\ 


"2(\8S+V^! 


M=MAq+  {Mj/+M„uq)  +Mwuw+-£-  fcnb(x)      ,  xq\    xdx-  (xJV-XgB)  cosG 

Q  w  q  w  2J         z  \W-Xq\ 

-{ZqW-z^)  sin8  +  u2  (Mb  6S+M6  bb) 


In  the  above  equations  is  the  vehicle  weight,  B  the  buoyancy, 
(xG,zG)  the  coordinates  of  the  center  of  gravity,  and  (xB,zB) 
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the  coordinates  of  the  center  of  buoyancy.  Also,  provision  for 
two  sets  of  control  surfaces  (stern  and  bow  planes)  is  made. 
The  kinematic  equations  are: 

x=ucos0+wsin0  (3.3) 

z=-usin6+wcos0  (3.4) 

6=g  (3.5) 

B.   CONTROL  LAW 

The    linearized   state    space    form   of    equations    (3.1), (3.2) 
and    (3.5)    is    used    for   vertical    plane   heading   control: 

w=a11uw+a12ug+al3Q+bllu2bs+bl2u2bb  (3.6) 

q=a21uw+a22uq+a23Q+b21u2bs+b22u2bb  (3.7) 


where : 


a 


Dv=(m-Zt)  {Iy-M4)-(mxG+Z^)  (mXe+Mj 


a±1  =  ^-  i  (Iy-Mj  Zw+  (mxc+zj  Mw] 
12  =  jr  I  (Iy"Mt)  (m+Zq)  +  (mxG+Zg)  (Mq-m)  ] 


20 


ai3  =  --~   [  (ZG"ZB)    (mXg  +  Z^)  Wl 


bn  =  -7T  [(Jy-Af^)z6s+(mxG+Z^)Af8s] 


D 


^12  =  3"   t  (Jy-^)  Zji+dlKo+Z^)^] 


a21=-^-[(m-ZI,)Mw+(7iucG+M^Zw] 


«a22  =  ^-  t  (ffl-^#)  (Afg-ffl)  +  Imxg+Mj  (m+Zq)  ] 


a23  =  ~~k-  Km-Zj  [zG-zB)w] 


b21  =  -±-[  (m-Z^Mbs+  (mxG+Mw)  Zbs\ 


b„  =  -4-  [(rn-ZjM,h+(mxn+MjZ,h] 


'2?. 


D 


w>  IJ6£>      ^UL*GT11*'  ^6i?J 


In  these  W=B  and  x6=xB  have  been  assumed.  Considering  that  the 
effect  of  the  bow  and  the  stern  planes  is  the  same  we  have: 


bs=b 


6^-6 
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so 


^1=^11-^12 


b2=b21-b22 


From   the   above   the    final    form   of    the    equations    of   motion 
is : 

w=a11uw+a12uq+a13Q+blu2b  (3.8) 

q=a21uw+a22uq+a226+b2u2b  (3.9) 

1.   ZERO  PITCH  ANGLE 

When  the  commanded  direction  of  the  underwater  vehicle  is 
horizontal  the  control  law  has  the  following  form: 

6=ic10+ic2^+ic3g  (3.10) 

where  k.,k2,k3  are  calculated  below.  From  the  system  of  the 
three  differential  equations  (3.5), (3.8), (3.9)  the  closed  loop 
characteristic  equation  has  the  following  form: 

A3+a1X2+a2X+a3  =  0  (3.11) 

where: 
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a1  =  -a11u-b1u2k2-a22u-b2u2k3 


^2^^iaZ2u2^aiXb2U^k^a22blu2K-ai2a21u2-b2aX2uZK-bxa21uZk^-a2--b2U 


a3=«313a21U-<313-b2u2^2-i:)ia21u3jci+aiia23U  +  <aili:?2u3;Cl+<a2.Au2;C2 
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Figure  4.  Vertical  plane  geometry:  Horizontal  commanded  path 
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The    desired    characteristic    polynomial    according    to    the    ITAE 
criterion    is: 

k3+a1k2+a2X+a3=0  (3.12) 

where: 

a1=l  .75u>0 


a2=2  .  15ci>o 


a3=co0 


u  -  10u 

L-vJ- 


and  t  represents  the  dimensionless  settling  time  for  the 
vertical  plane  autopilot.  Equating  the  coefficients  of 
equation    (3.11)    with   equation    (3.12)    we   get: 

b1u2k2+b2u2k3=-a1-  (<311+a22)  u  (3. 13) 

(bxa22-b2a12)  u*k2+{b2axl-b±a21)  u3ic3=a2+Jb2u2ic1 

+a23+(a12a21-alxa22)u2  (3.14) 

{^a^-b^^u^k^ia^b^a^b^u2^  (3.15) 

a3+(ai3a21-aiia23)  U 

To  simplify  notations,  equations  ( 3 . 13 ) , ( 3 . 14  )  and  (3.15)  are 
written  as: 
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where : 


A2k2+A,k,=Dx  (3.16) 

B1k1^B2k2+B,k2=D2  (3.17) 

C^+C.k^D,  (3*18) 


A2=bxuk 


A,=b2u2 


Bx=b2u2 
B2=  (bxa22-b2a12)  u2 

B3=(2?2a11-^1a21)u3 
q=  (b^-b^)  u2 
C2^^-^bx-a^b2)u2 
D1=-a1-  (-ai:L+a22)  u 

^2=a2+<323+  <ai2a21-aiia22)  U* 

D3=a3+(a13a21-a11a23)u 

From  the  above  system  of  equations  ( 3 . 16 ) , ( 3 . 17 ) , ( 3 . 18 )  we 
can  find  expressions  for  the  gains  k<|,k2,k3 
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K=     3      2   2  (3.19) 


fc>A,B,iVC1B,A-fl,CA  (3.20) 


A3  Bl  C2  +  Cl  S3  A2  "  Cl  A3  B2 


A, 


2.   NON  ZERO  PITCH  ANGLE 

When  the  commanded  pitch  angle  of  the  vehicle  is  not  equal 
to  zero,  we  have: 

Q=av+d'  (3.22) 

where:  av  is  the  commanded  pitch  angle 

0  '  is  the  deviation  from  the  commanded  angle 
Then 

sin0=sin<3„cos0/+cosa„sin0/=sina,. +0/cosa,. 

v  v  v  v 

for  small  deviations  0'.  The  system  of  equations  of  motion 
(3.5), (3.8), (3.9)  takes  the  form: 

0'=g  (3.23) 

w=a11uw+a12uq+al3cosavQf+b1u26+a13sinav  (3.24) 
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Figure  5.   Vertical  plane 


geometry:  Inclined  commanded  path' 
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q=a21uw+a22uq+a23cosavQ  +b2u2b+a22sinav  (3.25) 

The  control  law  now  takes  the  form: 

6=k1(e-av)+k2w+k3q+k4  (3.26) 

where  k^k^kj  can  be  calculated  with  the  some  procedure  as 
before,  and  the  feedforward  gain  kA  is  calculated  from  the 
desired  steady  state  accuracy.  At  steady  state  we  have: 

g=0 
Q=av 

e'=o 

so  that  the  system  of  the  equations  of  motion 
(3.23), (3.24), (3.25)  yields: 

a11uw+Jb1Li26+a13sinav=0  (3.27) 


a21uw+b2u26+a22sinav=0  (3.28) 


Equations  ( 3 . 27 ) , ( 3 . 28  )  can  be  solved  for  the  steady  state 
values  of  6  and  w,  and  by  substitution  into  equation  (3.26), 
after  some  calculations  kA  is  found  to  be: 
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a13(a21^Uic2)-a23(a11^lUic2)  (3>29) 


Note  that  if  a  =0  or  z  =zD  then  k,  =  0 


C.   GUIDANCE  LAW 

A  similar  to  the  horizontal  plane  case  guidance  law  can  be 
used  here  to  allow  path  keeping  in  the  vertical  plane.  To  the 
previous  system  of  differential  equations  (3.1), (3.2), (3.5) 
one  more  equation  is  added,  the  kinematic  equation  (3.4).  The 
new  system  is  now  going  to  be  examined  for  two  different 
cases . 

a)  Horizontal  path  (no  change  in  depth) 

b)  Inclined  path    (change  in  depth) 
1.   HORIZONTAL  PATH 

In  this  case  where  the  commanded  depth  remains  the  same 
the  control  law  is: 


b=k1(Q-Qc) +k2w+k3q  (3.30) 


where  0C  is  the  commanded  line  of  sight  (pitch  angle) 


e^tan1^-  (3.31) 
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where  k^,k2,k3  are  already  known  from  the  previous  section,  and 
xd  is  the  visibility  distance  similar  to  the  horizontal  case, 
shown  in  Figure  4 . 

2.   INCLINED  PATH 

Here  the  commanded  depth  changes  linearly  so  that  the 
angle  6  is  given  by: 

S=kx{$-av-&  c)  +k2w+k,q+kA  (3.32) 

where  k^k^k^k^  are  the  same  as  previously  determined. 
The  k4  term  exists  here  because  an  angle   6*0  has  to  remain 
when  the  underwater  vehicle  changes  depth  to  equalize  the 
restoring  moment  due  to  the  pitch  angle.  The  commanded  pitch 
angle  is: 

e^tarr1—  (3.33) 

xa 

where  z'  is  the  cross  track  error  off  the  inclined  path  as 
shown  in  Figure  5 . 

D.   STABILITY 

The  complete  system  is  given  by  the  equations  of  motion 
(3.1),  (3.2),  (3.4),  (3.5),  the  control  law  (3.30),  and  the 
guidance  law  (3.31).  Horizontal  motion  at  the  commanded  depth 
is  characterized  by: 

Q=w=q=z=0 
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Linearization  of  the  above  equations  produces  the  linear 
system: 

X=AX 


where 


and 


X=  [6,  w,q,  z] 


A  = 


ai2zGB+^Diu2Ki   a11u+jb1u2i(r2   a12u+Jb1u2if3    -bxu2 — - 

Xd 

a23zGB+b2u2Ki   a21u+£2u2JC2   a22u+b2u2K3    -b2u2-± 

Xd 


-U 


0 


0' 


(3.34) 


where : 


ZGB     ZG     ZB 


(3.35) 


is  the  metacentric  height.  Stability  properties  of  the 
straight  line  motion  are  established  by  the  eigenvalues  of 
matrix  [A] .  It  should  be  mentioned  that  from  now  until  the  end 
of  this  chapter  a13,  a23  have  been  redefined  to  show  explicitly 
the  metacentric  height  ZGB. 

A  program  is  written  to  compute  the  eigenvalues  of  matrix 
(3.34)  over  a  range  of  (t  ,x.)  values,  and  detect  whether  one 
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or  more  eigenvalues  become  unstable.  Typical  results  are  shown 
in  Figure  6  for  u=5  ft/sec  and  zGB=0.1. 
1.   REGIONS  OF  STABILITY 

It  can  be  seen  that  the  stability  boundary  of  Figure  6 
separates  the  parameter  space  (xd,tv)  into  three  regions: 

1:  Unstable  region,  one  pair  of  complex  conjugate 
eigenvalues  of  [A]  has  positive  real  parts. 

2:  Stable  region,  all  eigevalues  of  [A]  have  negative 
real  parts. 

3:  Unstable  region,  one  real  positive  eigenvalue  of 
[A]. 

Obviously,  stable  vehicle  response  is  not  possible  unless  the 
parameters  (xd,t  )  are  chosen  in  region  2. 
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1:  One  pair  of  complex  conjugate  eigevalues  with 

positive  real  parts. 
2:  Region  of  stability. 
3:  One  real  positive  eigenvalue. 
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Figure  6.   Regions  of  stability  for  u=5  ft/sec  and  zCB-0 . 1  ft 
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2.   SIMULATIONS 

Before  proceeding  further  with  the  stability  analysis, 
numerical  integrations  are  first  performed  in  order  to  examine 
the  response  of  the  vehicle  in  each  of  the  above  three  regions 
of  stability  of  Figure  6.  The  same  parameters  u=5  ft/sec  and 
zGB=0.1  ft  are  used.  Simulations  for  the  pitch  angle  0  and  the 
commanded  line  of  sight  angle  6c  for  the  case  tv=5  and  xd=4  are 
shown  in  Figure  7.  This  corresponds  to  region  2  of  Figure  6 
which  is  the  region  of  stability.  The  simulation  results  show 
that  the  actual  vehicle  pitch  angle  approaches  the  commanded 
angle,  after  some  oscillations,  and  the  depth  reaches  its 
commanded  value  at  zero  as  predicted. 

When  the  visibility  distance  is  xd=0.4  with  the  same  t  , 
the  vehicle  moves  into  the  unstable  region  1  of  Figure  6.  The 
simulated  response  is  shown  in  Figure  8  where  oscillatory 
characteristics  are  exhibited.  If  we  keep  the  same  value  for 
x.=4  and  we  change  the  controller  time  constant  t  =15  we  enter 

a  3  v 

the  unstable  region  3  of  Figure  6.  The  simulated  vehicle 
response  is  shown  in  Figure  9  where  it  appears  that  6  and  8C 
diverge  and  they  both  reach  nonzero  steady  state  values.  As  a 
result  the  vehicle  depth  is  now  a  linear  function  of  time, 
without  ever  stabilizing.  These  results  require  a  more 
detailed  analysis  of  the  regions  of  stability  of  the 
controller  /  guidance  combination. 
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Figure  7 .   Numerical  simulations  in  region  2 
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Figure   8.      Numerical   simulations   in  region   1 
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Regions   of    stability    for    zCB=0    and   for   any    speed 
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E.   ANALYSIS 

The  stability  properties  of  the  system  are  characterized 
by  the  eigenvalues  of  the  linearized  matrix  [A],  given  by 
equation  (3.34).  The  characteristic  equation  of  [A]  has  the 
form: 

A\4+Bk2+Ck2+Dk+E=0  (3.36) 

where : 

A  =  l 


B=a1 


,21-        1 


C=a2+b1u2k1  — 

xd 

D=a3+(b2a12-b±a22)  u2kx^-  -b2u2kL-±- 


Xd  Xd 


,2Zr      l-WKo      _^^>      \,t4;-      1 


E=  zGB (b2al3 -^a23 )u2k1—  +  (b^-b^^)  u4£x 


Xd  Xd 


According  to  Routh ' s  criterion  (3.36)  has  one  pair  of  complex 
conjugate  roots  crossing  the  imaginary  axis  when: 

BCD- AD2 -B2E=0  (3.37) 

After  some  algebra  ,  equation  (3.37)  can  be  written  as  : 
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a3(a1a2-a3)  x^+[d±  (a1a2-a3)  +a1c1a3-a3d1-<Xie1]     (3#38) 
xd+d1(a1c1-d1)  =0 


where : 

c1=A2k1 

d1=(-52-A3u)i:1 
e1=(-C2  +  C1u)ic1 

and  A2,  A3/ B2  C, , C2  were  defined  previously  following  equations 
(3. 16) , (3. 17)  and  (3.18). 

The  positive  root  of  equation  (3.38)  provides  the  critical 
value  of  xd  for  stability.  This  produces  the  curve  separating 
the  regions  1  and  2  of  Figure  6.  As  the  (xd,tv)  combinations 
cross  into  region  1,  the  response  of  the  system  becomes 
oscillatory  as  a  result  of  the  pair  of  complex  conjugate 
eigenvalues  with  positive  real  part.  This  explains  the 
simulations  observed  in  Figures  7  and  8. 

A  different  kind  of  instability  occurs  when  one  real  root 
of  (3.36)  crosses  zero.  For  this  to  happen  the  condition  is: 

F=0  (3.39) 

and  using  the  previous  definition  of  E,  this  happens  when 
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Jc^O  (3.40) 


Equations  (3.40)  and  (3.19)  yield 


A 


k2  =  -^-  (3.41) 


c2 


Equations  (  3  .  41 ) , ( 3 . 20 )  define  then  the  critical  condition  for 
stability.  In  our  case  this  can  be  simplified  as  follows.  The 
expression  for  C2  is  reproduced  here. 

^(a^b^a^b^z^u2  (3.42) 

Since  we  have  assumed  that  bow  and  stern  planes  have  the  same 
strength 

26s  =  26jb<0 


M6s  =  -M6b<0 


and  substituting  the  expressions  for  a23,b1 ,  a13,b2  we  can  find 
that 

C2  =  0  (3.43) 

Equations  (3.43)  and  (3.41)  then  require  that 


42 


£>3=0  (3.44) 


and  using  the  definition   for  D3  we   get 

tt3+  (ai3«S21-ail<323)  ZGBU  =  0 


or 


)    +  (a13a21  alxa22)  zGB-0 


tyl 


and  we  can  find  the  critical  value  of  t  as 

V 


t  10u 


vczitjcal  _1  (3.45) 

1  [  (<31i^23_ai3a2l)  ZGB^ 


Condition  (3.45)  shows  that  the  critical  value  of  t  is 
independent  of  xd  which  is  demonstrated  in  Figure  6  as  a 
straight  line  parallel  to  the  xd  axis.  Furthermore,  the  other 
stability  curve,  equation  (3.38),  intersects  the  t  axis  at 
xd=0  when  1^  =  0  which  is  the  same  condition  as  (3.45).  This 
means  that  the  stability  conditions  (3.38)  and  (3.45)  separate 
the  (xd,tv)  parameter  space  into  three  regions  of  stability, 
as  shown  in  Figure  6. 

Results  of  the  stability  regions  for  zGB=0  are  shown  in 
Figure  10.  These  are  independent  of  the  forward  speed  u  just 
as  in  the  horizontal  plane  case.  It  should  be  mentioned  that 
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for  zGB=0,  tvcriticaL  -»  oo  and  therefore  ,  region  3  of  figure  '6 
never  appears . 

For  z6B  >  0,  the  stability  regions  depend  heavily  on  the 
forward  speed  u.  This  is  demonstrated  in  Figure  11  for  zGB=0.1 
(ft)  and  various  values  of  u  in  (ft/sec).  As  the  speed  is 
decreased  the  critical  value  of  t  from  (3.45)  also  decreases 
with  the  effect  of  reducing  region  1  and  enlarging  region  2 
and   3 . 

The  effect  of  varying  the  metacentric  height  zGB  while 
keeping  u  constant  is  evaluated  in  Figure  12  for  u=2.  Similar 
conclusions  can  be  drawn  for  this  case  as  previously. 

The  critical  value  of  t  as  given  by  (3.45)  is  shown  in 
Figure  13  for  different  values  of  the  forward  speed  u  and  the 
metacentric  height  zGB.  The  surface  shown  in  the  figure 
separates  the  stability  regions  2  and  3. 

The  final  task  of  this  section  is  to  explain  the 
simulations  observed  in  Figure  9  when  the  vehicle  operates  in 
region  3 . 
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Figure  13.   Critical  value  of  tv   versus  u  and  z 


GB 
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F.   STEADY  STATE  SOLUTIONS 

It  was  shown  in  the  previous  section  that  transition 
between  Regions  2  and  3  is  associated  with  a  real  eigenvalue 
crossing  zero.  Usually  when  such  a  loss  of  stability  occurs 
and  the  primary  eguilibrium  solution  becomes  unstable, 
additional  stable  equilibrium  solutions  appear.  To  evaluate 
these  new  steady  state  solutions  we  consider  the  complete 
system  given  by  equations  (3.4),  (3.5),  (3.6),  (3.7),  (3.30), 
and  (3.31).  At  steady  state  the  time  derivatives  vanish  and  we 
get 

g=0  (3.46) 


v=_5ksin0  (3.47) 


b=D"   tt3sin6  (3.48) 

c 


Substituting  equations  ( 3 . 46 ) , ( 3  .  47  )  ,  and  (3.48)  into  equation 
(3.4)  we  get : 

(-u+^cos0)sin0  =  O  (3.49) 


Equation  (3.49)  may  accept  besides  the  normal  solution  6=0, 
another  solution  given  by: 
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q    (b2axx-bxa2X)  u2 

cos0  =  — -u  = — — — — (3.50) 

C2    (bxa23-b2aX3)  zGB 


Equation  (3.50)  is  valid  provided  cos0  <  1  which  means: 

U2<     ib^a23-b2^12^GB  (3.51) 

b2axx-bxa2x 


If   (3.51)   is   satisfied  the  equilibrium  angle  0  can  be 
determined  from  (3.50)  provided: 


A -a 


3— ^sin0<6sa£  (3.52) 


where  6sat  is  the  maximum  dive  plane  angle  typically   set  at 
0  .  4  radians . 

In  our  case  conditions  (3.51)  and  (3.52)  are  not 
satisfied,  which  means  that  the  non  zero  equilibrium  pitch 
angle  cannot  be  computed  from  (3.50).  Furthermore  z  *  0  at 
steady  state,  which  means  that  z=constant.  Therefore,  z  is 
linearly  increasing  with  time,  and 

tan-1  —  -—  ,as.  .  .  t-~  (3.53) 

xd     2 
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Substituting  equations  (3.46),  (3.47),  (3.48),  and  (3.53)  into 
the  control  law  (3.30)  and  (3.31)  we  can  find  the  equation  for 
the  unknown  steady  state  pitch  angle. 

(D3-a3)  sin6  =  ^:1C1  (6-—)  +Jc2C2sin6         (3.54) 

If  we  call  6  -  n/2   =6',  equation  (3.54)  reduces  to: 

ic1C10/+(a3-ic1C1)cos0/=O  (3.55) 

It  can  now  be  seen  that  equation  (3.55)  has  a  solution  when  It- 
crosses  zero  which  is  the  same  condition  for  transition 
between  regions  (2)  and  (3)  found  in  the  previous  section.  The 
steady  state  solution  is  then  computed  from  (3.55)  if: 

3sin6^6sac  (3.56) 


Ck 


or  from 


sin6=  ^Cl  fi  ,  (3.57) 


L>3-a3 


otherwise . 

Results  for  the  steady  values  of  8  and  6  are  presented  in 
Figures  14  and  15  versus  zGB  for  u=5  and  tv=15. 
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Solid  lines  correspond  to  stable  and  dashed  lines  to  unstable 
equilibrium  positions.  It  can  be  seen  that  the  simulation 
results  for  ZGB  =0.1  of  Figure  9  are  verified. 

The  steady  state  pitch  angle  6=0  loses  its  stability  at 
ZGB=0.07  and  begins  to  increase  together  with  the  dive  plane 
angle  6.  This  is  up  to  ZGB=0.12  where  6  reaches  its  maximum 
value.  For  increasing  ZGB  beyond  this  point,  the  pitch  angle 
6  begins  to  decrease  since  6  remains  constant.  These  results 
are  for  fixed  t  and  u.  Results  for  different  values  of  the 
controller  settling  time  t  and  vehicle  speed  u  are  shown  in 
Figures  16  and  17  respectively. 
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Figure   14.      Steady   state  pitch  angle   6  versus   zCB 
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IV  THREE  DIMENSIONAL  GUIDANCE  CONTROL 

The  horizontal  and  vertical  plane  guidance  and  control 
laws  that  were  developed  in  the  previous  two  chapters  are 
combined  here  to  provide  accurate  path  keeping  in  three 
dimensional  space.  The  other  requirement  is  that  the  forward 
speed  along  the  path  should  be  constant  and  equal  to  a 
commanded  value.  This  will  enable  path  tracking  instead  of 
simply  path  keeping. 

A.   PROPULSION  CONTROL 

Just  as  the  horizontal  (vertical)  plane  path  control 
design  was  based  on  the  linearized  lateral  (vertical) 
equations  of  motion,  the  propulsion  control  law  will  be  based 
on  the  linearized  form  of  the  surge  equation  only.  The  surge 
equation  is: 

mu=X,u+X^w2  +  uw(Xwb-Xw6)b+u2(Xb.b)62  +  CDo(a2n2-u2)     (4.1) 


where : 


g=  Ujnax  (4.2) 

■'•Tnax 


n  is  the  propeller  revolutions,  and  6  the  dive  plane  angle. 
Only  w  and  6  terms  remain  in  equation  (4.1)  because  only  these 
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terms  are  nonzero  at  steady  state  for  a  constant  commanded 
dive  or  rise  angle.  A  propulsion  control  law  is  introduced  of 
the  form: 

n=n0+kn(u-uc)  (4.3) 

The  feedback  gain  kn  is  computed  from  stability 
requirements  whereas  the  feedforward  term  nQ  is  computed  from 
steady  state  accuracy.  When  n=nQ  the  forward  speed  u  must 
equal  the  commanded  speed  u  .  Therefore,  (4.1)  becomes: 


f(uc)+CDa2n{;=0  (4.4) 


where  we  defined 

f(u)  =Xwww2  +  UW(Xw6-Xusb)  6+u2  UM+X6*)  ?>2-CDu2  (4.5) 


The  terms  w  and  6  are  given  as  functions  of  uc  and  the 
commanded  pitch  angle  av  by 

w=     (^22~b2ai^  ZGBSinav  (4#6) 

(311252-a21i)1)  uc 


(4.7) 


Solving  (4.4)  for  n0  we  get 
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2     f(Uc) 

n0=-——  (4.8) 

CDna 


This  term  nQ  guarantees  the  required  steady  state  accuracy.  To 
evaluate  kn  we  substitute  (4.3)  and  (4.8)  into  (4.1)  and  we 
get : 

(m-X0)  u-2CDa2n0kn(u-uc)  =0  (4.9) 


The  characteristic  equation  of  (4.9)  is 


m-X, 


The  desired  characteristic  equation  is 


S  +  G)0  =  0,  .  ,fa>0=    ^c  (4.11) 


where  t  is  the  desired  dimensionless  settlinq  time  for  the 

n  3 

speed  control.  Comparing  (4.10)  with  (4.11)  we  can  solve  for 
the  control  gain. 

K=~  2        *  (4.12) 
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With  the  choice  of  gains  (4.12)  and  (4.8),  the  propulsion 
control  law  (4.3)  is  complete. 

B.   THREE  DIMENSIONAL  PATH  KEEPING 

Suppose  the  commanded  path  is  a  general  straight  line  in 
three  dimensions,  from  point  0  to  point  F  as  shown  in  Figure 
18.  The  vehicle  position  is  at  point  A.  With  respect  to  the 
inertial  coordinate  frame  (x,y,z)  the  commanded  path  is 
characterized  with  the  two  angles  aH  and  av  as  shown  in  the 
Figure.  In  order  to  achieve  the  commanded  path,  a  coordinate 
frame  rotation  by  an  angle  aH  is  performed  first  as  shown  in 
Figure  19.  The  necessary  geometric  relations  are: 


a^tarr1-^-^ 

XF    Xo 


(4.13) 


x'=  (y-y0)  sinaH+  (x-x0)  cosaH  (4.14) 

y'=  {y-y0)  cosaH-  (x-xQ)  sinaH  (4.15) 

The  rudder  control  law  is  then  of  the  form: 

b=k1(y-aH-oH)  +k2v+k2r  (4.16) 

where  the  line  of  sight  angle  for  horizontal  plane  control  aH 
is  defined  by: 
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y' 
tano„=-^—  (4.17) 


xdH  is  the  lookahead  distance  determined  according  to  the 
stability  analysis  of  Chapter  II,  and  k1 ,  k2,  k3  are  the 
horizontal  plane  control  gains  from  Chapter  II. 

Another  rotation  by  an  angle  av  is  conducted  next  as  shown 
in  Figure  20.  The  geometric  relations  here  are: 

ay=tan"x  Zjr~z°  (4.18) 

x' F=  (yF-yQ)  sinaw+  (xF-x0)  cosaK  (4.19) 

x"=-  (z-z0)  sinav+x'cosav  (4  .20) 

z/=(z-zc)cosav+x/sinav  (4.21) 

The   dive   plane    control    law   is: 

5=k1(Q-av-ov)  +k2w+k3q+k4  (4.22) 

where  the  line  of  sight  angle  for  vertical  plane  control  av  is 
defined  by: 

z' 
tanav= 


xc 
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xdu  is  the  lookahead  distance  determined  according  to  the 
stability  analysis  of  Chapter  III,  and  k1 ,  k2,  k3,  k4  are  the 
vertical  plane  control  gains  as  computed  in  Chapter  III.  The 
existence  of  two  distinct  distances  x.u,  x.   is  for  maximum 

an '    av 

flexibility  in  the  design  and  to  allow  for  the  possibly 
different  stability  conditions  for  horizontal  and  vertical 
plane,  as  analyzed  in  the  previous  two  chapters. 

Results  are  presented  for  a  typical  three  dimensional 
commanded  route  that  consists  of  the  following  way  points  (x, 
y,  z)  =  (20,  0,  5),  (40,  5,  5),  (60,  -5,  -3),  (100,  0,  -5) 
vehicle  lengths  with  individual  straight  line  paths  connecting 
them.  Switch  from  one  to  the  next  straight  line  path  was 
initiated  when  the  vehicle  position,  measured  along  the 
current  commanded  path,  was  within  a  specified  target  distance 
(TD)  from  the  way  point.  Parameters  used  for  the  simulation 
were  the  following:  tH=7,  t  =5,  zG=0.1,  tN=0.2,  xdH=3,  xdv=2.5 
commanded  speeds  u=(4,  4,  5,  5)  for  the  four  straight  line 
segments  respectively,  and  TD=1.  Simulation  results  are 
presented  in  Figure  21  through  25.  It  can  be  seen  from  Figures 
21  that  accurate  path  control  is  maintained  in  both  the 
horizontal  and  vertical  planes.  Speed  control  is  also  very 
accurate,  see  Figure  22,  despite  the  course  changes  and 
nonzero  dive  and  rise  angles.  The  speed  controller  revolutions 
per  minute  are  shown  in  Figure  23,  where  the  maximum 
saturation  limit  is  set  500  rpm.  Rudder  response  is  shown  in 
Figure  25  where  the  steady  state  nonzero  values  occur  during 
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a  nonzero  commanded  pitch  angle.  Comparing  Figures  24  and  25 
with  22,  it  can  be  observed  that  the  vehicle  slows  down 
momentarily  when  the  control  surfaces  become  active,  a 
situation  which  is  quickly  corrected  by  the  speed  controller. 
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Figure  18.   Coordinate  transformation  for  3-D  path  keeping 
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Figure  19.   Horizontal  plane  rotation 
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Figure  20.   Vertical  plane  rotation 
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Figure  21.   Numerical  simulation  for  3-D  path  keeping 
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CONCLUSIONS  AND  RECOMMENDATIONS 

The  main  conclusions  and  contributions  of  this  work  can  be 
summarized  as  follows: 

1.  Pursuit  guidance  law  and  decoupled  horizontal  and 
vertical  plane  orientation  controllers  were  shown  to  provide 
accurate  vehicle  path  keeping  in  three  dimensions. 

2.  The  scheme  proved  to  be  robust  enough  so  that  it  could 
handle  the  nonlinear  coupling  between  speed  response,  and 
horizontal  and  vertical  plane  motions  without  performance 
degradation. 

3.  It  was  shown  that  the  guidance  and  control  schemes  must 
be  designed  together  in  order  to  avoid  loss  of  stability  or 
excessive  oscillatory  response. 

4.  Analytic  conditions  for  stability  were  derived.  The 
conditions  were  expressed  explicitly  in  terms  of  the  vehicle 
hydrodynamic  characteristics  and  the  guidance  and  control  law 
design  specifications. 

5.  An  extensive  study  of  the  mechanism  of  loss  of 
stability  was  undertaken  for  the  vertical  plane  motions.  Two 
distinct  possibilities  were  discovered  and  analyzed.  In  the 
first  one  pair  of  complex  conjugate  eigenvalues  crosses  the 
imaginary  axis  and  results  in  an  oscillatory  vehicle  behavior 
around  the  commanded  path.  In  the  second  ,  one  real  eigenvalue 
crosses  zero  and  the  vehicle  drifts  off  to  a  steady  state  path 
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with  its  deviation  from  the  commanded  path  linearly  increasing 
with  time.  This  new  path  was  computed  and  explicit  conditions 
to  avoid  such  a  undesirable  situation  were  given. 

Some  recommendations  for  further  research  include  the 
following: 

1.  Comparisons  from  the  point  of  view  of  path  keeping 
response  under  physical  system  /  mathematical  model  mismatch. 

2.  State  estimators  must  be  included  in  the  analysis  to 
evaluate  performance  under  partial  state  knowledge  and  sensor 
noise . 
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APPENDIX  A 


C      PROGRAM  SIM_3D.FOR 

C 

C      FOTIS  A.  PAPOULIAS/ANGELLOS  G.  PAPASOTIRIOU 

C      NAVAL  POSTGRADUATE  SCHOOL 

C      AUGUST  1991 

C 

C      VEHICLE  THREE  DIMENSIONAL  PATH  KEEPING 

C      HEADING  AUTOPILOTS 

C      PURE  PURSUIT  NAVIGATION 

C      SIMULTANEOUS  RUDDER/DIVE  PLANE  SWITCHINGS 

C 

C      DECLARATIONS 

C 

REAL  L, MASS, IX, IY, IZ,IXZ, IYZ,IXY 

REAL  K1H,K2H,K3H,K1V,K2V,K3V,K4V,KN 

REAL  KPDOT , KRDOT , KPQ , KQR , KVDOT , KP , KR , KVQ , KWP , KWR , KV , KVW , 
&      KPN,KDB 

REAL  MQDOT , MPP , MPR , MRR , MWDOT , MQ , MVP , MVR , MW , MVV , 
&      MDS,MDB,NDRB 

REAL  NPDOT , NRDOT , NPQ , NQR , NVDOT , NP , NR , NVQ , NWP , NWR , 
&      NV,NVW,NDRS 

REAL  MM(6,6) , INDX(IOO) 

DIMENSION  X(9) ,BR(9) ,HH(9) ,VECH1(9) ,VECH2(9) , XMMINV( 6 , 6 ) 

DIMENSION  VECV1(9) ,VECV2(9) , F( 12 ) , FP( 6 ) ,DISV( 100 ) 

DIMENSION  XDES(IOO) , YDES(IOO) ,ZDES(100) ,UDES(100) , 
&  DISH(IOO) 

C 

C      GEOMETRIC  PROPERTIES 
C 

WEIGHT=12000.0 


IX 

= 

1760.0 

IY 

= 

9450.0 

IZ 

L0700.0 

IXY 

= 

0.0 

IYZ 

= 

0.0 

IXZ 

= 

0.0 

L 

= 

17.425 

RHO 

= 

1.94 

G 

= 

32.2 

XG 

= 

0.0 

YG 

= 

0.0 

XB 

= 

0.0 

YB 

= 

0.0 

ZB 

= 

0.0 
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c 

c 
c 


c 

c 

c 


AO     =     2.0 
CDO    =     0.0057 
MASS   =WEIGHT/G 
BOY    =WEIGHT 
RPMMAX=   500.0 
RPMMIN=  -500.0 
UMAX   =     6.0 
UMIN   =     0.1 
ALPHA  =UMAX/RPMMAX 

SURGE  HYDRODYNAMIC  COEFFICIENTS 


XPP  =  7.030E-03*0 
XQQ  =-1.470E-02*0 
XRR  =  4.010E-03*0 
XPR  =  7.640E-04*0 
XUDOT  =-7.580E-03*0 
XWQ  =-1 .920E-01*0 
XVP  =-3.240E-03*0 
XVR  =  1.890E-02*0 
XQDS  =  2.610E-02*0 
XQDB  =-2.600E-03*0 
XRDR  =-8.180E-04*0 
XW  =  5.290E-02*0 
XWW  =  1.710E-01*0 
XVDR  =  1.730E-03*0 
XWDS  =  4.600E-02*0 
XWDB  =  9.660E-03*0 
XDSDS  =-1.160E-02*0 
XDBDB  =-8.070E-03*0 
XDRDR  =-1 .010E-02*0 
XRES  =  CD0*0 
XPROP  =  XRES*ALPHA* 


.5*RHO 

.5*RHO 
.5*RHO 
•5*RHO 
•5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
.5*RHO 
*2 


*L**4 
*L**4 

*L*  *4 

*L*  *4 

*L**3 

*L**3 

*L**3 

*L**3 
*L*  *  3 

*L**3 
*L**3 

*L**2 
*L**2 
*L**2 
*L**2 
*L**2 
*L**2 
*L**2 
*L**2 
*L**2 


SWAY  HYDRODYNAMIC  COEFFICIENTS 


YPDOT 

=  1, 

.270E- 

-04*0, 

.5*RHO*L**4 

YRDOT 

=  1, 

.240E- 

-03*0, 

, 5*RHO*L**4 

YPQ 

=  4, 

.125E- 

-03*0, 

,5*RHO*L**4 

YQR 

=  -6, 

.510E- 

-03*0, 

,5*RHO*L**4 

YVDOT 

=  -5, 

.550E- 

-02*0, 

,5*RHO*L**3 

YP 

=  3 

.055E- 

-03*0, 

.5*RHO*L**3 

YR 

=  2, 

.970E- 

-02*0, 

.5*RHO*L**3 

YVQ 

=    2, 

.360E- 

-02*0, 

,5*RHO*L**3 

YWP 

=  2, 

.350E- 

-01*0, 

,5*RHO*L**3 

YWR 

=  -1. 

.880E- 

-02*0, 

, 5*RHO*L**3 

YV 

=  -9, 

.310E- 

-02*0, 

,5*RHO*L**2 

YVW 

=  6, 

.840E- 

-02*0, 

,5*RHO*L**2 

YDRS 

=+2, 

.270E- 

-02*0, 

,5*RHO*L**2 

YDRB 

=  +  2, 

.270E- 

-02*0, 

,5*RHO*L**2 
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c 
c 


HEAVE  HYDRODYNAMIC  COEFFICIENTS 


C 
C 
C 


C 

c 
c 


c 
c 
c 


ZQDOT 

=  • 

-6 

.810E- 

-03*0, 

. 5*RHO*L**4 

ZPP 

= 

1 

.270E- 

-04*0 

. 5*RHO*L**4 

ZPR 

= 

6 

.670E- 

-03*0, 

.5*RHO*L**4 

ZRR 

=  . 

-7, 

.350E- 

-03*0, 

. 5*RHO*L**4 

ZWDOT 

=  . 

-2 

.430E- 

-01*0, 

, 5*RHO*L**3 

ZQ 

=  ■ 

-1, 

.350E- 

-01*0, 

,5*RHO*L**3 

ZVP 

=  . 

-4, 

.810E- 

-02*0, 

,5*RHO*L**3 

ZVR 

= 

4, 

.550E- 

-02*0, 

,5*RHO*L**3 

ZW 

=  . 

-3, 

.020E- 

-01*0, 

,5*RHO*L**2 

ZW 

=  - 

-6, 

.840E- 

-02*0, 

.5*RHO*L**2 

ZDS 

=  . 

-2, 

■270E- 

-02*0, 

,5*RHO*L**2 

ZDB 

=  - 

-2, 

.270E- 

-02*0, 

, 5*RHO*L**2 

ROLL  HYDRODYNAMIC  COEFFICIENTS 


KPDOT 

=  - 

-1, 

.010E- 

-03*0, 

,5*RHO*L**5 

KRDOT 

=- 

-3, 

.370E- 

-05*0, 

,5*RHO*L**5 

KPQ 

=  ■ 

-6, 

.930E- 

-05*0, 

. 5*RHO*L**5 

KQR 

= 

1, 

.680E- 

-02*0, 

.5*RHO*L**5 

KVDOT 

= 

1, 

.270E- 

-04*0, 

,5*RHO*L**4 

KP 

=  . 

-1, 

.100E- 

-02*0, 

,5*RHO*L**4 

KR 

=  - 

-8, 

.410E- 

-04*0, 

,5*RHO*L**4 

KVQ 

=  - 

-5, 

-115E- 

-03*0, 

,5*RHO*L**4 

KWP 

=  - 

-1, 

.270E- 

-04*0, 

,5*RHO*L**4 

KWR 

= 

1, 

.390E- 

-02*0, 

,5*RHO*L**4 

KV 

= 

3, 

.055E- 

-03*0, 

.5*RHO*L**3 

KVW 

=  - 

-1, 

.870E- 

-01*0, 

,5*RHO*L**3 

PITCH  HYDRODYNAMIC  COEFFICIENTS 


MQDOT 

MPP 

MPR 

MRR 

MWDOT 

MQ 

MVP 

MVR 

MW 

MW 

MDS 

MDB 


680E- 
260E- 
040E- 
860E- 
810E- 
860E- 
180E- 
730E- 
860E- 
510E- 
113E- 
113E- 


02*0 
05*0 
03*0 
03*0 
02*0 
02*0 
03*0 
02*0 
02*0 
02*0 
02*0 
02*0 


5*RHO* 
5*RHO* 
5*RHO* 
5*RHO* 
5*RHO* 
5*RHO* 
5*RHO* 
5*RHO* 
5*RHO* 
5*RHO* 
5*RHO* 
5*RHO* 


L**5 
L**5 
L**5 
L**5 
L**4 
L**4 
L**4 
L**4 
L**3 
L**3 
L**3 
L**3 


YAW  HYDRODYNAMIC  COEFFICIENTS 


NPDOT  =-3 
NRDOT  =-3 
NPQ  =-2 
NQR    =  2 


370E-05*0 
400E-03*0 
110E-02*0 
750E-03*0 


5*RHO*L**5 
5*RHO*L**5 
5*RHO*L**5 
5*RHO*L**5 


75 


c 
c 
c 


c 
c 
c 


c 
c 
c 


NVDOT 

=  1 

-240E- 

-03*0 

5*RHO*L**4 

NP 

=  -8 

.405E- 

-04*0 

5*RHO*L**4 

NR 

=  -1 

. 640E- 

-02*0 

,5*RHO*L**4 

NVQ 

=  -9 

-990E- 

-03*0 

5*RHO*L**4 

NWP 

=  -1 

.750E- 

-02*0 

5*RHO*L**4 

NWR 

=  7 

.350E- 

-03*0 

-5*RHO*L**4 

NV 

=  -7 

.420E- 

-03*0 

-5*RHO*L**3 

NVW 

=  -2 

-670E- 

-02*0 

5*RHO*L**3 

NDRS 

=  -1 

-113E- 

-02*0 

5*RHO*L**3 

NDRB 

=+1 

. 113E- 

-02*0 

5*RHO*L**3 

OPEN  DATA  AND  RESULTS  FILES 


' PATH_3D . DAT ' , STATUS= ' OLD 
' XY . RES ' , STATUS= ' NEW ' ) 
' XZ . RES ' , STATUS= ' NEW ' ) 
' DRS . RES ' , STATUS= ' NEW ' ) 
' DS . RES ' , STATUS= ' NEW ' ) 
' YCTE . RES ' , STATUS= ' NEW ' ) 
' ZCTE . RES ' , STATUS= ' NEW ' ) 
' XYZ . RES ' , STATUS= ' NEW ' ) 
' U . RES ' , STATUS= ' NEW ' ) 
1 RPM . RES '  , STATUS=  *  NEW '  ) 
' PHI . RES ' , STATUS= ' NEW ' ) 
' THETA . RES ' , STATUS= ' NEW ' ) 
•PS I. RES' ,STATUS='NEW' ) 
' V . RES ' , STATUS= ' NEW ' ) 
•R.RES' , STATUS='NEW' ) 
•W.RES ' ,STATUS='NEW' ) 
' Q . RES ' , STATUS= ' NEW ' ) 
' YZ . RES ' , STATUS= ' NEW ' ) 


READ  DATA  FILE 

READ  (10,*)  TS IM, DELTA, IPRNT 
READ  (10,*)  IPTS, TARGET 
READ  (10,*)  TN,TH,TV,ZG 
IF  (IPTS. GT. 100)  IPTS=100 
DO  1  1=1, IPTS 

READ  (10,*)  XD,YD,ZD,XDH,XDV,UO 

XDES(I)=XD*L 

YDES(I)=YD*L 

ZDES(I)=ZD*L 

UDES(I)=U0 

DISH( I)=XDH*L 

DISV(I)=XDV*L 
CONTINUE 

MASS  MATRIX  INITIALIZATION  AND  DEFINITION 


OPEN  i 

;io 

-FILE 

OPEN 

11 

,  FILE 

OPEN  | 

;i2 

-FILE 

OPEN  | 

;i3 

rFILE 

OPEN  | 

,14 

FILE 

OPEN  | 

,15 

-FILE 

OPEN  | 

'16 

-FILE 

OPEN  | 

,17 

-FILE 

OPEN  i 

,18 

-FILE 

OPEN  i 

'19 

-FILE 

OPEN  i 

,20 

FILE 

OPEN  i 

,21 

-FILE 

OPEN  | 

[22, 

-FILE 

OPEN  | 

23 

FILE 

OPEN  | 

,24 

-FILE 

OPEN  i 

'25 

-FILE 

OPEN  i 

,26 

FILE 

OPEN  | 

,27 

-FILE 

DO  15  J=l,6 
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DO  10  K=l,6 

XMMINV( J,K)=0.0 
MM(J,K)=0.0 

0    CONTINUE 

5  CONTINUE 


C 

c 
c 


c 
c 
c 


MASS-XUDOT 
MASS*ZG 
-MASS*YG 

MASS-YVDOT 
-MASS*ZG-YPDOT 
MASS*XG-YRDOT 

MASS-ZWDOT 
MASS*YG 
-MASS*XG-ZQDOT 

-MASS*ZG-KVDOT 

MASS*YG 

IX-KPDOT 
•IXY 
-IXZ-KRDOT 

MASS*ZG 
-MASS*XG-MWDOT 
-IXY 

IY-MQDOT 
■IYZ 

■MASS*YG 

MASS*XG-NVDOT 
-IXZ-NPDOT 
■IYZ 

IZ-NRDOT 


MASS  MATRIX  INVERSION 

DO  12  1=1,6 
DO  11  J=l,6 

XMMINV(I, J) =0.0 

1  CONTINUE 
XMMINV(I, I)=1.0 

2  CONTINUE 

CALL  INVTA(MM,6, INDX,D) 
DO  13  J=l,6 

CALL  INVTB(MM,6,INDX,XMMINV(1, J) ) 

3  CONTINUE 

VARIABLES  INITIALIZATION 


MM 

[1 

-I) 

MM 

;i 

,5) 

MM 

;i 

r6) 

MM 

[2 

,2) 

MM 

[2 

>4) 

MM 

[2 

,6) 

MM 

[3 

,3) 

MM( 

;3J 

4) 

MMi 

;3, 

5) 

MM| 

;4, 

2) 

MM| 

[4, 

3) 

MMl 

[4, 

4) 

MM| 

[4, 

5) 

MMl 

At 

6) 

MM) 

5, 

1) 

MMl 

:s, 

3) 

MM( 

5/ 

4) 

MM( 

5, 

5) 

MM| 

5; 

6) 

MM( 

6/ 

1) 

MM( 

6, 

2) 

MM< 

,6, 

4) 

MM< 

>6, 

5) 

MM| 

,6, 

6) 
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PISIM 

=TSIM/DELTA 

ISIM 

=PISIM 

ECHO 

=1.0 /DELTA 

IECHO 

=ECHO 

YAW 

=  0.0 

SWAY 

=  0.0 

PITCH 

=  0.0 

HEAVE 

=  0.0 

U 

=UDES(1) 

RPM 

=UDES(1) /ALPHA 

V 

=  0.0 

W 

=  0.0 

P 

=  0.0 

Q 

=  0.0 

R 

=  0.0 

DS 

=  0.0 

DB 

=  0.0 

DR 

=  0.0 

TWOPI 

=8.0*ATAN(1.0) 

PI 

=0.5*TWOPI 

PHI 

=  0.0 

ISTART=1 

TARGET=TARGET*L 

XPOS 

=  0.0 

YPOS 

=  0.0 

ZPOS 

=  0.0 

CDY 

=  0.5 

CDZ 

=  0.5 

JPRNT 

=  0 

UK 

=  0 

JE 

=  0 

DRS 

=  0.0 

DRB 

=  0.0 

DS 

=  0.0 

DB 

=  0.0 

c 

c 

DEFINE  THE  LENGTH  X, 

c 

X(l) 

=   -105.9/12.0 

X(2) 

-99.3/12.0 

X(3) 

-87.3/12.0 

X(4) 

-66.3/12.0 

X(5) 

72.7/12.0 

X(6) 

83.2/12.0 

X(7) 

91.2/12.0 

X(8) 

99.2/12.0 

X(9) 

103.2/12.0 

c 

HH(1) 

0.00/12.0 

HH(2) 

8.24/12.0 

HH(3) 

19.76/12.0 
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c 
c 
c 


c 
c 

c 


HH(4 
HH(5 
HH(6 
HH(7 
HH(8 
HH(9 

BR(1 
BR(2 
BR  (3 
BR  (4 
BR(5 
BR(6 
BR(7 
BR(8 
BR  (9 


29 

31 
27 

21 
12 


36/12 
85/12 

84/12 
44/12 
00/12 


0.00/12 


0 

8 

19 

29 

31 
27 
21 
12 
0 


00/12 
24/12 
76/12 
36/12 
85/12 
84/12 
44/12 
00/12 
00/12 


AUXILLIARY  VARIABLES  FOR  HORIZONTAL  PLANE  CONTROL 


DH   = 

A11H= 
A12H= 

A21H= 
A22H= 

B11H= 

B12H= 

B21H= 

B22H= 

B1H 

B2H 


IZ-NRDOT)*(MASS-YVDOT)- 
MASS*XG-YRDOT)*(MASS*XG-NVDOT) 
( I Z -NRDOT ) * YV- ( MASS*XG- YRDOT ) *NV ) /DH 
( I Z -NRDOT ) * ( -MAS  S+YR ) - 
MASS*XG-YRDOT) * ( -MASS*XG+NR) ) /DH 
(MASS-YVDOT) *NV-(MASS*XG-NVDOT)*YV)/DH 
(MASS-YVDOT) * ( -MASS*XG+NR) - 
MASS*XG-NVDOT)*(-MASS+YR) ) /DH 
(IZ-NRDOT)*YDRS-(MASS*XG-YRDOT)*NDRS)/DH 
( IZ-NRDOT) *YDRB- (MASS*XG- YRDOT) *NDRB) /DH 
(MASS-YVDOT) *NDRS-(MASS*XG-NVDOT)*YDRS)/DH 
(MASS-YVDOT) *NDRB- (MASS*XG-NVDOT) *YDRB) /DH 

B11H-B12H 

B21H-B22H 


AUXILLIARY  VARIABLES  FOR  VERTICAL  PLANE  CONTROL 


DV  = 
A11V= 
A12V= 
A13V= 
A21V= 
A22V= 
A2  3V= 
B11V= 
B12V= 
B21V= 
B22V= 
B1V  = 
B2V  = 


MASS-ZWDOT) *(IY-MQDOT)-ZQDOT*MWDOT 

( I Y-MQDOT ) *  ZW+ZQDOT*MW ) /DV 

( I Y-MQDOT ) * ( ZQ+MASS ) +ZQDOT*MQ) /DV 

- ( ZG-ZB) * (MASS*XG+ZQDOT) *WEIGHT/DV 
MWDOT*ZW+ (MASS-ZWDOT) *MW)/DV 
MWDOT* ( ZQ+MASS ) + (MASS-ZWDOT) *MQ) /DV 

- ( ZG-ZB ) * (MASS-ZWDOT) *WEIGHT/DV 
(IY-MQDOT)*ZDS+ZQDOT*MDS)/DV 
( IY-MQDOT) *ZDB+ZQDOT*MDB ) /DV 
MWDOT*ZDS+(MASS-ZWDOT) *MDS ) /DV 
MWDOT*ZDB+(MASS-ZWDOT)*MDB)/DV 

B11V-B12V 

B21V-B22V 


C 
C 
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C      SIMULATION  BEGINS 

C 

C      LOOP  OVER  WAY  POINTS 

C 

DO  200  IP=1, IPTS 

IF  (IP.GE.2)  GO  TO  210 

XDH=DISH(1) 

XDV=DISV(1) 

U0  =UDES(1) 

XD  =XDES(1) 

YD  =YDES(1) 

ZD  =ZDES(1) 

XD1=0.0 

YD1  =  0.0 

ZD1=0.0 

XD2=XD 

YD2=YD 

ZD2=ZD 

GO  TO  211 

210  XDH=DISH(IP) 
XDV=DISV(IP) 
U0  =UDES(IP) 
XD  =XDES(IP) 
YD  =YDES(IP) 
ZD  =ZDES(IP) 
XD1=XD2 
YD1=YD2 
ZD1=ZD2 
XD2=XD 
YD2=YD 
ZD2=ZD 

211  ZD12=ZD2-ZD1 
XD12=XD2-XD1 
YD12=YD2-YD1 

C 

C         HORIZONTAL  HEADING  CONTROL  GAINS 

C 

OMEGAH=( 10.0*U0)/(TH*L) 

AD1H=1.75*0MEGAH 

AD2H=2 . 15*OMEGAH**2 

AD3H=OMEGAH**3 

A1=B1H*U0*U0 

B1=B2H*U0*U0 

C1=-AD1H-(A11H+A22H)*U0 

A2=(B1H*A22H-B2H*A12H)*U0**3 

B2=(B2H*A11H-B1H*A21H)*U0**3 

KlH=AD3H/( (B2H*A11H-B1H*A21H)*U0**3) 

C2=AD2H-(A11H*A22H-A12H*A21H)*U0**2+B2H*U0*U0*K1H 

K2H=(C1*B2-C2*B1)/(A1*B2-A2*B1) 

K3H=(C2*A1-C1*A2)/(A1*B2-A2*B1) 
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C        VERTICAL  HEADING  CONTROL  GAINS 
C 

OMEGAV= ( 1 0 . 0*U0 ) / ( TV*L ) 

AD1V=1.75*0MEGAV 

AD2V=2 . 15*OMEGAV**2 

AD3V=OMEGAV**3 

A2=B1V*U0*U0 

A3=B2V*U0*U0 

D1=-AD1V-(A11V+A22V) *U0 

B1=-B2V*U0*U0 

B2=(B1V*A22V-B2V*A12V)*U0**3 

B3=(B2V*A11V-B1V*A21V)*U0**3 

D2=AD2V+A2  3V+(A12V*A21V-A11V*A22V)*U0**2 

C1=(B2V*A11V-B1V*A21V)*U0**3 

C2=(A2  3V*B1V-A13V*B2V)*U0**2 

D3=AD3V+(A13V*A21V-A11V*A2  3V)*U0 

K2V=(A3*B1*D3+C1*B3*D1-D2*C1*A3) 

K2V=K2V/(A3*B1*C2+C1*B3*A2-C1*A3*B2) 

K1V=(D3-C2*K2V)/C1 

K3V=(D1-A2*K2V)/A3 
C 

ALPHAH= ATAN ( YD 1 2 /XD 1 2 ) 

ALPHAH=ABS ( ALPHAH ) 

IF  ( (XD12.GE.0.0) .AND. (YD12.GE.0.0) )  ALPHAH=   ALPHAH 

IF  ( (XD12.GE. 0.0)  .AND.  (YD12.LT. 0.0) )  ALPHAH=  -ALPHAH 

IF  ( (XD12.LT.0.0) .AND. (YD12 .GE. 0 . 0  )  )  ALPHAH=PI -ALPHAH 

IF  ( (XD12.LT.0.0) .AND. ( YD12 . LT . 0 . 0 ) )  ALPHAH=PI+ALPHAH 

XCTEH= ( YPOS-YD1 ) *SIN ( ALPHAH ) + ( XPOS-XD1 ) *COS ( ALPHAH ) 

YCTE  =(YPOS-YDl)*COS (ALPHAH ) - (XPOS-XD1 ) *SIN( ALPHAH) 

X1P   =YD12 * S IN ( ALPHAH )+XD12*COS( ALPHAH) 

ALPHAV=ATAN (ZD12/X1P) 

ALPHAV=ABS ( ALPHAV ) 

IF  (ZD12.GE.0.0)  ALPHAV= -ALPHAV 

K4V=-(A13V*(A21V+B2V*U0*K2V)-A2  3V*(A11V+B1V*U0*K2V) ) 

K4V=K4V*SIN(ALPHAV)/( (B1V*A21V-B2V*A11V) *U0*U0 ) 

ZCTE  =  (ZPOS-ZDl)*COS(ALPHAV)+XCTEH*SIN(ALPHAV) 

XCTEV=-(ZPOS-ZDl)*SIN( ALPHAV) +XCTEH*COS (ALPHAV) 
C 

C        PROPULSION  CONTROL  GAIN 
C 

WSS=(B1V*A2  3V-B2V*A13V)* SIN (ALPHAV) 

WSS=WSS/( (A11V*B2V-A21V*B1V)*U0) 

DSS=(A21V*A13V-A11V*A2  3V)*SIN( ALPHAV) 

DSS=DSS/ ( (A11V*B2V-A21V*B1V) *U0*U0 ) 

FUC=XWW*WSS**2+U0*WSS*(XWDS-XWDB)*DSS 
&      +U0*U0*(XDSDS+XDBDB)*DSS**2-XRES*U0**2 

RPM0=-FUC/(XRES*ALPHA**2) 

RPM0=SQRT(RPM0) 

WRITE  (*,*)  RPM0,U0/ALPHA 

KN=-5.0*U0*(MASS-XUDOT)/(XRES*ALPHA*ALPHA*RPM0*TN*L) 
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WRITE  (*,201)  XD/L,YD/L,ZD/L 
C 

C        SIMULATION  FOR  EACH  WAY  POINT 
C 

DO  100  I=ISTART, ISIM 
ICOUNT=I 
C 

IF  (U.LT.UMIN)  U=UMIN 
C 

C  CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER 

C  THE  VEHICLE 

C 

DO  600  K=l,9 

UCF=(V+X(K)*R)**2+(W-X(K)*Q)**2 
UCF=SQRT(UCF) 

IF  (UCF.LT.l.E-6)  GO  TO  601 
CFLOW=CDY*HH(K)*(V+X(K)*R)**2+CDZ*BR(K)* 
&  (W-X(K)*Q)**2 

VECH 1 ( K ) =CFLOW* ( V+X ( K ) *R ) /UCF 
VECH2 ( K ) =CFLOW* ( V+X ( K ) *R ) *X ( K ) /UCF 
VECV1 (K)=CFLOW* (W-X(K) *Q) /UCF 
VECV2(K)=CFLOW*(W-X(K)*Q)*X(K)/UCF 

600  CONTINUE 

CALL  TRAP ( 9, VECV1,X, HEAVE) 
CALL  TRAP(9,VECV2,X,PITCH) 
CALL  TRAP (9, VECH 1,X, SWAY  ) 
CALL  TRAP(9/VECH2/X, YAW   ) 
HEAVE=-0 . 5*RHO*HEAVE 
PITCH=+0 . 5*RHO*PITCH 
SWAY  =-0.5*RHO*SWAY 
YAW   =-0.5*RHO*YAW 
GO  TO  602 

601  HEAVE=0.0 
PITCH=0.0 
SWAY  =0.0 
YAW   =0.0 

602  CONTINUE 
C 

C  FORCE  EQUATIONS 

C 

c 

C  SURGE  FORCE 

C 

FP(1)  =  MASS*V*R-MASS*W*Q+MASS*XG*Q**2+MASS*XG*R**2- 

&  MASS*YG*P*Q-MASS*ZG*P*R+XPP*P**2+XQQ* 

&  Q**2+XRR*R**2+XPR*P*R+XWQ*W*Q+XVP*V*P+ 

&  XVR*V*R+U*Q*(XQDS*DS+XQDB*DB)+ 

&  U*R*(XRDRS*DRS+XRDRB*DRB)+XW*V**2+XWW* 

&  W**2+U*V*(XVDRS*DRS+XDRB*DRB)+U*W* 

&  (XWDS*DS+XWDB*DB)+(XDSDS*DS**2+XDBDB*DB**2+ 

&  XDRDR*(DRS**2+DRB**2) ) *U**2- (WEIGHT-BOY) * 
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&  SIN(THETA)+XPROP*RPM*RPM-XRES*U*U 

C 

C  SWAY  FORCE 

C 

FP(2)  =-MASS*U*R-MASS*XG*P*Q+MASS*YG*R**2-MASS*ZG*Q*R+ 

&  YPQ*P*Q+YQR*Q*R+YP*U*P+YR*U*R+YVQ*V*Q+ 

&  YWP*W*P+YWR*W*R+YV*U*V+YVW*V*W+YDRS*U**2*DRS+ 

&  YDRB*U**2*DRB+(WEIGHT-BOY)* 

&  COS(THETA)*SIN(PHI)+MASS*W*P+MASS*YG*P**2+SWAY 

C 

C  HEAVE  FORCE 

C 

FP(3)  =  MASS*U*Q-MASS*V*P-MASS*XG*P*R-MASS*YG*Q*R+ 

&  MASS*ZG*P**2+MASS*ZG*Q**2+ZPP*P**2+ 

&  ZPR*P*R+ZRR*R**2+ZQ* 

&  U*Q+ZVP*V*P+ZVR*V*R+ZW*U*W+ZW*V**2+HEAVE  + 

&  U**2*(ZDS*DS+ZDB*DB)+ (WEIGHT-BOY)* 

&  COS(THETA)*COS(PHI) 

C 

C  ROLL  MOMENT 

C 

FP(4)  =  -IZ*Q*R+IY*Q*R-IXY*P*R+IYZ*Q**2- 

&  IYZ*R**2+IXZ*P*Q+MASS*YG*U*Q-MASS* 

&  YG*V*P-MASS*ZG*W*P+KPQ*P*Q+KQR*Q*R+ 

&  KP*U*P+KR*U*R+KVQ*V*Q+KWP*W*P+ 

&  KWR*W*R+KV*U*V+KVW*V*W+(YG*WEIGHT-YB*BOY)* 

&  COS ( THETA ) *COS ( PHI ) - ( ZG*WEIGHT- 

&  ZB*BOY)*COS( THETA) *SIN( PHI ) +MASS*ZG*U*R 

C 

C  PITCH  MOMENT 

C 

FP(5)  =  -IX*P*R+IZ*P*R+IXY*Q*R-IYZ*P*Q- 

&  IXZ*P**2+IXZ*R**2-MASS*XG*U*Q+ 

&  MASS*XG*V*P+MASS*ZG*V*R- 

&  MASS*ZG*W*Q+MPP*P**2+ 

&  MPR*P*R+MRR*R**2+MQ* 

&  U*Q+MVP*V*P+MVR*V*R+MW*U*W+ 

&  MW*V**2+U**2*  (MDS*DS+MDB*DB)-(XG*WEIGHT- 

&  XB*BOY)*COS(THETA)*COS(PHI)- 

&  ( ZG*WEIGHT-ZB*BOY) *S IN ( THETA) +PITCH 

C 

C  YAW  MOMENT 

C 

FP(6)  =  -IY*P*Q+IX*P*Q+IXY*P**2-IXY*Q**2+IYZ*P*R- 

&  IXZ*Q*R-MASS*XG*U*R+MASS*XG*W*P-MASS*YG* 

&  V*R+MASS*YG*W*Q+NPQ*P*Q+NQR*Q*R+NP*U*P+NR* 

&  U*R+NVQ*V*Q+NWP*W*P+NWR*W*R+NV*U*V+ 

&  NVW*V*W+NDRS*U**2*DRS+NDRB*U**2*DRB+ 

&  (XG*WEIGHT-XB*BOY )*COS( THETA) *S IN (PHI) + 

&  (YG*WEIGHT-YB*BOY)*SIN(THETA)+YAW 
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c 

C  COMPUTE  THE  RIGHT  HAND  SIDE  OF  XDOT=F(X) 

C 

DO  610  J  =  1,6 
F(J)  =  0.0 
DO  611  K  =  1,6 

F(J)  =  XMMINV(J,K)*FP(K)  +  F(J) 
611        CONTINUE 
610      CONTINUE 
C 

C  INERTIAL  POSITION  RATES 

C 

F(7)  =  U*COS(PSI)*COS(THETA)+V*(COS(PSI)*SIN(THETA)* 
&  SIN(PHI)-SIN(PSI)*COS(PHI) ) +W* ( COS ( PS I ) * 

&  S IN ( THETA ) *COS ( PHI )+SIN(PSI)*SIN( PHI) ) 

C 

F(8)  =  U*SIN(PSI)*COS(THETA)+V*(SIN(PSI)*SIN(THETA)* 
&  SIN(PHI)+COS(PSI)*COS(PHI) ) +W* ( SIN( PS I ) * 

&  S IN ( THETA ) *COS ( PHI ) -COS (PSI)*SIN( PHI) ) 

C 

F(9)  =  -U*SIN(THETA)+V*COS(THETA)*SIN(PHI)+ 
&  W*COS(THETA)*COS(PHI) 

C 

C  EULER  ANGLE  RATES 

C 

C 

c 


F ( 1 0 ) =  P+Q*  S IN ( PH I ) *TAN ( THETA ) +R*COS ( PH I ) *TAN ( THETA ) 

F(ll)=  Q*COS(PHI)-R*SIN(PHI) 

F ( 1 2 ) =  Q*  S IN ( PHI ) /COS ( THETA ) +R*COS ( PHI ) /COS ( THETA ) 


C 

C  ASSIGN  XDOT  VECTOR 

C 


UDOT 

= 

F(l) 

VDOT 

= 

F(2) 

WDOT 

= 

F(3) 

PDOT 

= 

F(4) 

QDOT 

= 

F(5) 

RDOT 

= 

F(6) 

XDOT 

= 

F(7) 

YDOT 

= 

F(8) 

ZDOT 

= 

F(9) 

PHIDOT  =  F(10) 
THEDOT  =  F(ll) 
PSIDOT  =  F(12) 


c 

c 

FIRST 

ORDER 

INTEGRATION 

c 

U 

=  U 

+  DELTA*UDOT 

V 

=  V 

+  DELTA*VDOT 

w 

=  W 

+  DELTA*WDOT 
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p 

= 

P 

+ 

DELTA*PDOT 

Q 

= 

Q 

+ 

DELTA*QDOT 

R 

= 

R 

+ 

DELTA* RDOT 

XPOS 

= 

XPOS 

+ 

DELTA*XDOT 

YPOS 

= 

YPOS 

+ 

DELTA*YDOT 

ZPOS 

= 

ZPOS 

+ 

DELTA* ZDOT 

PHI 

= 

PHI 

+ 

DELTA*PHIDOT 

THETA 

= 

THETA 

+ 

DELTA*THEDOT 

PSI 

= 

PSI 

+ 

DELTA*PSIDOT 

c 

C  VELOCITY  INPUT  CALCULATION 

C 

uc=uo 

IF  (UC.GE.UMAX)  UC=UMAX 

IF  (UC.LE.UMIN)  UC=UMIN 
C 

C  RPM  INPUT  CALCULATION 

C 

RPMO=UC/ALPHA 

RPM=RPMO+KN* ( U-UC ) 

IF  ( RPM . GE . RPMMAX )  RPM=RPMMAX 

IF  (RPM.LE.RPMMIN)  RPM=RPMMIN 
C 

C  COORDINATE  TRANSFORMATIONS 

C 

XCTEH=  ( YPOS- YD1 ) *  S IN ( ALPHAH )  +  ( XPOS-XD1 ) *COS ( ALPHAH ) 

YCTE  =  (YPOS-YDl)*COS(ALPHAH)-(XPOS-XDl)*SIN(ALPHAH) 

ZCTE  =  (ZPOS-ZDl)*COS(ALPHAV)+XCTEH*SIN(ALPHAV) 

XCTEV=-(ZPOS-ZDl)*SIN(ALPHAV)+XCTEH*COS(ALPHAV) 
C 

C  HIT  CRITERIA 

C 

VT0TAL=(XD2-XD1)**2+(ZD2-ZD1)**2 

VTOTAL=  SQRT ( VTOTAL ) 

HT0TAL=(XD2-XD1)**2+(YD2-YD1)**2 

HTOTAL=SQRT ( HTOTAL ) 

VAWAY  =VTOTAL-XCTEV 

VAWAY  =ABS( VAWAY) 

HAWAY  =HTOTAL-XCTEH 

HAWAY  =ABS( HAWAY) 

IF  ( (VAWAY. LT. TARGET) .OR. (HAWAY. LT. TARGET ) )  GO  TO 
&        101 
C 

C  DIVE  PLANE  INPUT  CALCULATION 

C 

ZPHI=ZCTE 

SIGV=ATAN(ZPHI/XDV) 

DS=K1V*(THETA-ALPHAV-SIGV)+K2V*W+K3V*Q+K4V 
C 

IF  (DS.GE.  0.4)  DS=  0.4 

IF  (DS.LE.-0.4)  DS=-0.4 
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c 

c 
c 


DB=-DS 

RUDDER  INPUT  CALCULATION 

YPHI=YCTE 

S IGH=-ATAN ( YPH I /XDH ) 

DRS=K1H*(PSI-ALPHAH-SIGH)+K2H*V+K3H*R 

IF  (DRS.GE.  0.4)  DRS=  0.4 
IF  (DRS.LE.-0.4)  DRS=-0.4 


c 
c 
c 

DRB=-DRS 

PRINT  RESULTS 

TIME=I*DELTA 

JE=JE+1 

IF  (JE 

NE. IECHO)  GO  TO  99 

JE  =  0 

99 

JPRNT=JPRNT+1 

IF  (JPRNT.NE 

. IPRNT)  GO  TO  100 

IJK=IJK+1 

TIME=I*DELTA 

WRITE 

[H#*) 

XPOS/L,YPOS/L 

WRITE  | 

;i2,*) 

XPOS/L,ZPOS/L 

WRITE 

,13,*) 

TIME,DRS*180.0/PI 

WRITE  I 

[14,*) 

TIME,DS*180.0/PI 

WRITE 

;i5,*) 

TIME,YCTE/L 

WRITE 

;i6/*) 

TIME,ZCTE/L 

WRITE 

;i7,*) 

XPOS/L^POS/LfZPOS/L 

WRITE 

;i8,*) 

TIME,U 

WRITE 

;i9,*) 

TIME,RPM 

WRITE 

;20,*) 

TIME/PHI*180.0/PI 

WRITE 

;2i,*) 

TIME, (THETA-ALPHAV)*180.0/PI 

WRITE 

;22,*) 

TIME, (PSI-ALPHAH)*180.0/PI 

WRITE 

[23,*) 

TIME,V 

WRITE 

[24, *) 

TIME,R 

WRITE 

;25,*) 

TIME,W 

WRITE 

;26,*) 

TIME,Q 

WRITE 

;27 ,*) 

YPOS/L,ZPOS/L 

JPRNT=( 

) 

100  CONTINUE 
GO  TO  500 

101  ISTART=ICOUNT 

200  CONTINUE 
500  STOP 

201  FORMAT  ('  HEADING  FOR  (X,Y,Z) 
&         \F9.31   )') 

END 


=  (  ' ,F9.3, '  ,  ' ,F9.3, 
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c 
c= 

c 

c 
c 
c 


SUBROUTINE  TRAP ( N, A, B , OUT ) 

NUMERICAL  INTEGRATION  ROUTINE  USING  THE  TRAPEZOIDAL  RULE 

DIMENSION  A(l) ,B(1) 

N1=N-1 

OUT=0.0 

DO  1  1=1, Nl 

OUT1=0.5*(A(I)+A(I+1) )*(B(I+1)-B(I)) 

OUT  =OUT+OUTl 
1  CONTINUE 
RETURN 
END 
C 

c 

SUBROUTINE  INVTA(MM,N, INDX,D) 

PARAMETER  (NMAX=100 , TINY=1 . OE-20 ) 

DIMENSION  INDX(6)  ,W(NMAX) 

REAL  MM(6,6) 

D=l 

DO  12  1  =  1, N 

AAMAX=0. 

DO  11  J=1,N 

IF(ABS(MM(I/ J) ) .GT.AAMAX)  AAMAX=ABS (MM( I , J ) ) 

11  CONTINUE 

IF  (AAMAX.EQ.O. )  PAUSE  ' SINGULAR  MATRIX ' 
W(I)  =  1./AAMAX 

12  CONTINUE 

DO  19  J=1,N 

DO  14  1=1, J-l 
SUM=MM(I, J) 
DO  13  K=l, 1-1 

SUM=SUM-MM ( I ,  K ) *MM ( K , J ) 

13  CONTINUE 
MM(I, J)=SUM 

14  CONTINUE 
AAMAX=0. 

DO  16  I=J,N 
SUM=MM(I, J) 
DO  15   K=1,J-1 

SUM=  SUM-MM ( I , K ) *MM ( K , J ) 

15  CONTINUE 
MM(I, J)=SUM 
DUM=W(I)*ABS(SUM) 

IF  ( DUM . GE . AAMAX )  THEN 
IMAX=I 
AAMAX=DUM 
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ENDIF 

16  CONTINUE 

IF  (J.NE.IMAX)THEN 
DO  17  K=1,N 

DUM=MM(IMAX,K) 

MM(IMAX/K)=MM(J/K) 

MM(J,K)=DUM 

17  CONTINUE 
D=-D 
W(IMAX)=W(J) 

ENDIF 

INDX(J)=IMAX 

IF(MM(J/ J) .EQ.O. )MM(J, J)=TINY 

IF( J.NE.N)THEN 

DUM=1 ./MM (J, J) 

DO  18  I=J+1,N 

MM(I,J)=MM(I,J)*DUM 

18  CONTINUE 
ENDIF 

19  CONTINUE 
RETURN 
END 


SUBROUTINE  INVTB(MM, N, INDX, B) 

DIMENSION  INDX(N),B(N) 

REAL  MM ( 6 , 6 ) 

11  =  0. 

DO  12  1=1, N 

LL=INDX(I) 

SUM=B(LL) 

B(LL)=B(I) 

IF  (II.NE.O)THEN 

DO  11  J=II,I-1 

SUM=SUM-MM( I, J)*B( J) 

11  CONTINUE 

ELSE  IF  (SUM.NE.O)  THEN 

11  =  1 
ENDIF 
B(I)=SUM 

12  CONTINUE 

DO  14  I=N,1,-1 
SUM=B(I) 
IF  (I.LT.N)THEN 
DO  13  J=I+1,N 

SUM=SUM-MM( I , J ) *B ( J ) 

13  CONTINUE 
ENDIF 
B(I)=SUM/MM(I, I) 

14  CONTINUE 
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RETURN 
END 
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APPENDIX  B 


C 
C 

c 
c 
c 
c 
c 
c 


PROGRAM  VERT_STAB.FOR 

REGIONS  OF  STABILITY  -  VERTICAL  PLANE 
PARAMETERS  ARE:  XD  AND  TV 
NUMERICAL  OR  ANALYTIC  COMPUTATION 

IT  NEEDS  FILE  " SUBRTNS . FOR"  OR  ANY  STANDARD  EIGENVALUE 

SOLVER 

IMPLICIT  DOUBLE  PRECISION  (A-H,0-Z) 
DOUBLE  PRECISION  K1V, K2V, K3V, L 

DOUBLE  PRECISION  MQDOT , MQ , MW, MWDOT , MDS , MDB , MASS , I Y 
DIMENSION  A(4,4) ,FV1(4) , IV1(4),ZZZ(4,4),WR(4),WI(4) 


OPEN  ( 

10,FILE='BIF0 

.RES' , STATUS= 

'NEW' 

) 

OPEN  ( 

11,FILE= 'BIF1 

.RES' , STATUS= 

'NEW' 

) 

OPEN  ( 

12,FILE='BIF2 

.RES' , STATUS= 

'NEW' 

) 

OPEN  ( 

13,FILE='BIF3 

.RES' , STATUS= 

'NEW' 

) 

WEIGH! 

1=12000.0 

IY 

=  9450.0 

L 

17.425 

RHO 

1.94 

G 

32  .2 

XG 

0.0 

ZB 

0.0 

MASS 

=WEIGHT/G 

BOY 

=WEIGHT 

ZQDOT 

=-6.810E-03*0 

,5*RHO*L**4 

ZWDOT 

=-2.430E-01*0 

,5*RHO*L**3 

ZQ 

=-1.350E-01*0 

,5*RHO*L**3 

zw 

=-3.020E-01*0 

. 5*RHO*L**2 

ZDS 

=-2.270E-02*0 

,5*RHO*L**2 

ZDB 

=-2.270E-02*0 

. 5*RHO*L**2 

MQDOT 

=-1.680E-02*0 

,5*RHO*L**5 

MWDOT 

=-6.810E-02*0 

. 5*RHO*L**4 

MQ 

=-6.860E-02*0 

.5*RHO*L**4 

MW 

=  9.860E-02*0 

,5*RHO*L**3 

MDS 

=-1.113E-02*0 

.5*RHO*L**3 

MDB 

=  1.113E-02*0 

,5*RHO*L**3 

WRITE  (*,1001) 

READ   (*,*)     TVMIN,TVMAX, ITV 

WRITE  (*,1002) 


READ   ( 


) 


XDMIN,XDMAX, IXD 
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XDMIN=XDMIN*L 

XDMAX=XDMAX*L 

WRITE  (*,1003) 

READ   (*,*)     U,ZG 

WRITE  (*,1004) 

READ   (*,*)     ISOL 

C 

C      AUXILIARY  VARIABLES 

C 


DV   = 
A11V= 

A12V= 


A13V=- ( ZG-ZB) * (MASS*XG+ZQDOT) *WEIGHT/DV 


A21V= 
A22V= 


MASS-ZWDOT)*(IY-MQDOT)-ZQDOT*MWDOT 

( I Y-MQDOT ) *  ZW+ZQDOT*MW ) /DV 

( IY-MQDOT) * ( ZQ+MASS ) +ZQDOT*MQ ) /DV 


MWDOT*ZW+(MASS-ZWDOT)*MW)/DV 
MWDOT* ( ZQ+MASS ) + (MASS- ZWDOT ) *MQ ) /DV 


A2  3V=- ( ZG-ZB) * (MASS- ZWDOT) *WEIGHT/DV 
Bl 1V= ( ( IY-MQDOT ) *  ZDS  +  ZQDOT*MDS ) /DV 
B12V=( ( IY-MQDOT) *ZDB+ZQDOT*MDB)/DV 
B21V=( MWDOT* ZDS+(MASS-ZWDOT)*MDS)/DV 
B22V=(MWDOT*ZDB+(MASS-ZWDOT)*MDB)/DV 
B1V  =B11V-B12V 
B2V  =B21V-B22V 
C 

EPS   =l.D-5 
ILMAX=1500 
C 

C      LOOP  OVER  TV 
C 

DO  1  1=1, ITV 

WRITE  (*,2001)  I, ITV 

TV=TVMIN+ ( I - 1 ) * ( TVMAX-TVMIN ) / ( ITV- 1 ) 

OMEGAV= (10.0*U)/(TV*L) 

AD1V=1.75*0MEGAV 

AD2V=2 . 15*OMEGAV**2 

AD3V=OMEGAV**3 

A2=B1V*U*U 

A3=B2V*U*U 

D1=-AD1V- ( A11V+A22V) *U 

B1=-B2V*U*U 

B2=(B1V*A22V-B2V*A12V)*U**3 

B3=(B2V*A11V-B1V*A21V)*U**3 

D2=AD2V+A2  3V+(A12V*A21V-A11V*A22V)*U**2 

C1=(B2V*A11V-B1V*A21V)*U**3 

C2=(A2  3V*B1V-A13V*B2V)*U**2 

D3=AD3V+(A13V*A21V-A11V*A23V)*U 

K2V=(A3*B1*D3+C1*B3*D1-D2*C1*A3) 

K2V=K2V/(A3*B1*C2+C1*B3*A2-C1*A3*B2) 

K1V=(D3-C2*K2V)/C1 

K3V=(D1-A2*K2V)/A3 

D3  3  3=(A13V*A21V-A11V*A2  3V)*U 

XAAA=CBRT(-D333) 
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c 
c 
c 


c 
c 
c 
c 


22 


C 

c 


D334=(D3*B2*C1*A3+B3*C1*A2*A3-B3*C1*D1*C2-D1*C1*C2*A3) 

D335=B3*C1*A2+B2*C1*A3-B1*C2*A3 

D336=D334/D335 

XBBB=CBRT(D336) 

ANALYTICAL  COMPUTATION 

IF  (ZG.NE.0.0)  TVCR1=(10.*U)/(XAAA*L) 

IF  (ZG.NE.0.0)  TVCR2=(10.*U)/(XAAA*L) 

IF  (ISOL.EQ.O)  GO  TO  22 

CXD2=AD3V* ( AD1V*AD2V-AD3V) 

CXD1=-(B2+A3*U)*K1V*(AD1V*AD2V-AD3V)+AD3V*K1V* 

(A2*AD1V+B2+A3*U)-AD1V*AD1V*K1V*(-C2+C1*U) 
CXD0=-(B2+A3*U)*K1V*K1V*(AD1V*A2+B2+A3*U) 
DET=CXDl*CXDl-4 . 0*CXD2*CXD0 
IF  (DET.LT.0.0)  GO  TO  1 
XD1= ( -CXD1+DSQRT ( DET ) ) / ( 2 . 0*CXD2 ) 
XD2= ( -CXDl-DSQRT(DET) ) / ( 2 . 0*CXD2 ) 
IF  (XD1.NE.0.0) 

VALl=AD3V+( (B2V*A12V-B1V*A22V-B2V)*K1V*U**3)/XD1 
IF  (XD2.NE.0.0) 

VAL2=AD3V+( ( B2V*A12V-B1V*A22V-B2V) *K1V*U**3 ) /XD2 
GO  TO  2  3 

NUMERICAL  COMPUTATION 
LOOP  OVER  XD 

DO  2  J=1,IXD 

XD=XDMIN+ ( J- 1 ) * ( XDMAX-XDMIN ) / ( IXD- 1 ) 

THETA=0.0D0 

CT=DCOS(THETA) 

ST=DSIN(THETA) 

W=0.0D0 

A(l, 1)=0.0D0 

A(1/2)=0.0D0 

A(1,3)=1.0D0 

A(1,4)=0.0D0 

A(2, 1 )=B1V*U*U*K1V+A13V*CT 

A(  2 , 2 ) =B1V*U*U*K2V+A1 1V*U 

A( 2 , 3 ) =B1V*U*U*K3V+A12V*U 

A( 2  ,  4 ) =-BlV*U*U*KlV/XD 

A(3,  1)=B2V*U*U*K1V+A23V*CT 

A( 3 , 2 ) =B2V*U*U*K2V+A2 1V*U 

A(3/3)=B2V*U*U*K3V+A22V*U 

A(  3 , 4 ) =-B2V*U*U*KlV/XD 

A(4/1)=-U*CT-W*ST 

A(4,2)=CT 

A(4,3)=0.0D0 

A(4,4)=0.0D0 

COMPUTE  EIGENVALUES 
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CALL  RG ( 4 , 4 , A , WR , WI , 0 , Z  Z  Z , I VI , FV1 , IERR ) 
CALL  DSTABL(DEOS,WR,WI,FREQ) 

IF  ( J.GT. 1)  GO  TO  10 
DEOSOO=DEOS 
XDOO   =XD 
LL=0 
GO  TO  2 
10      DEOSNN=DEOS 
XDNN   =XD 
PR=DEOSNN*DEOSOO 
IF  (PR.GT.0.D0)  GO  TO  3 
LL=LL+1 

IF  (LL.GT.3)  STOP  1000 
IL=0 

XDO=XDOO 
XDN=XDNN 
DEOSO=DEOSOO 
DEOSN=DEOSNN 
6      XDL=XDO 
XDR=XDN 
DEOSL=DEOSO 
DEOSR=DEOSN 
XD=(XDL+XDR)/2 


DO 


A( 

1, 

1) 

A( 

1, 

2) 

A( 

1, 

3) 

A( 

1, 

4) 

A( 

2; 

1) 

A( 

2, 

2) 

A( 

2, 

3) 

A( 

2, 

4) 

A( 

3, 

1) 

A( 

3, 

2) 

A( 

3; 

3) 

A( 

3; 

4) 

A( 

4, 

1) 

A( 

4, 

2) 

A( 

4, 

,3) 

A( 

4, 

-4) 

=  0 
=  0 

=  1 

=  0 


0D0 
0D0 
0D0 
0D0 


=B1V*U*U*K1V+A13V*CT 

=B1V*U*U*K2V+A11V*U 

=B1V*U*U*K3V+A12V*U 

=-BlV*U*U*KlV/XD 

=B2V*U*U*K1V+A2  3V*CT 

=B2V*U*U*K2V+A21V*U 

=B2V*U*U*K3V+A22V*U 

=-B2V*U*U*KlV/XD 

=-U*CT-W*ST 

=CT 

=0.0D0 

=0.0D0 

CALL  RG(4,4,A,WR,WI,0,ZZZ,IV1,FV1, IERR) 
CALL  DSTABL(DEOS,WR,WI,FREQ) 

DEOSM=DEOS 

XDM=XD 

PRL=DEOSL*DEOSM 

PRR=DEOSR*DEOSM 

IF  (PRL.GT.O.DO)  GO  TO  5 

XDO=XDL 
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XDN=XDM 
DEOSO=DEOSL 
DEOSN=DEOSM 
IL=IL+1 

IF  (IL.GT. ILMAX)  STOP  3100 
DIF=DABS(XDL-XDM) 
IF  (DIF.GT.EPS)  GO  TO  6 
XD=XDM 
GO  TO  4 

IF  (PRR.GT.0.D0)  STOP  3200 
XDO=XDM 
XDN=XDR 
DEOSO=DEOSM 
DEOSN=DEOSR 
IL=IL+1 

IF  ( IL.GT. ILMAX)  STOP  3100 
DIF=DABS(XDM-XDR) 
IF  (DIF.GT.EPS)  GO  TO  6 
XD=XDM 
LLL=10+LL 

WRITE  (LLL,*)  XD/L,TV 
XDOO=XDNN 
DEOSOO=DEOSNN 
CONTINUE 


GO  TO  1 

23    IF  (VAL1 

IF  (VAL2 

1  CONTINUE 

IF  (ZG.NE. 

IF  (ZG.NE. 


GT.0.0) 
GT.0.0) 


WRITE 
WRITE 


(11/*) 
(12,*) 


XD1/L,TV 
XD2/L,TV 


0.0) 
0.0) 


WRITE 

WRITE 


(10,*) 
(10,*) 


XDMIN/L,TVCR1 
XDMAX/L,TVCR1 


1001 
1002 
1003 
1004 
J 
2001 


FORMAT 
FORMAT 
FORMAT 
FORMAT 

FORMAT 
END 


( 


ENTER 
ENTER 
ENTER 
ENTER 


(215) 


MIN, 
MIN, 
U  AND 


MAX, 

MAX, 

ZG' 


AND 
AND 


0  :  NUMERICAL' 

1  :  ANALYTICAL 


INCREMENTS 
INCREMENTS 

) 


OF 

OF 


TV 
XD 


SUBROUTINE  DSTABL ( DEOS , WR , WI , OMEGA ) 
IMPLICIT  DOUBLE  PRECISION  (A-H,0-Z) 
DIMENSION  WR(4),WI(4) 
DEOS=-1.0D+20 
DO  1  1=1,4 

IF  (WR(I) .LT.DEOS)  GO  TO  1 

DEOS=WR(I) 

IJ=I 
CONTINUE 
OMEGA=WI(IJ) 
OMEGA=DABS ( OMEGA ) 
RETURN 
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END 

FUNCTION  CBRT(A) 

IF  (A.GT.0.0)  CBRT=    A  **(l./3.) 

IF  (A.LE.0.0)  CBRT=-(-A)**(l./3. ) 

RETURN 

END 
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APPENDIX  C 


C 

c 
c 
c 
c 
c 


PROGRAM  VERT_STEADY.FOR 

COMPUTATION  OF  STEADY  STATE  SOLUTIONS  IN  THE  VERTICAL 

PLANE 
(CHAPTER  III,  PARAGRAPH  F) 

REAL  K 1 V , K2 V , K3V , L , MQDOT , MQ , MW , MWDOT , MDS , MDB , MAS S , I Y 


WEIGHT=12000.0 

IY 

=  9450.0 

L 

17.425 

RHO 

1.94 

G 

32.2 

XG 

0.0 

ZB 

0.0 

MASS 

=WEIGHT/G 

BOY 

=WEIGHT 

ZQDOT 

=-6.810E-0  3*0.5*RHO*L**4 

ZWDOT 

=-2.4  30E-01*0.5*RHO*L**3 

ZQ 

=-1 .  350E-01*0.5*RHO*L**3 

zw 

=-3.02  0E-01*0.5*RHO*L**2 

ZDS 

=  -2 .270E-02*0.5*RHO*L**2 

ZDB 

=  -2  .  270E-02*0.5*RHO*L**2 

MQDOT 

=  -1  .  68  0E-02*0.5*RHO*L**5 

MWDOT 

=-6.810E-02*0.5*RHO*L**4 

MQ 

=-6.8  60E-02*0.5*RHO*L**4 

MW 

=  9.860E-02*0.5*RHO*L**3 

MDS 

=-1. 113E-02*0.5*RHO*L**3 

MDB 

=  1.113E-02*0.5*RHO*L**3 

OPEN 

(11/FILE=,THETA1.RES' , STATUS= 

' NEW  *  ) 

OPEN 

( 12 ,FILE= ' THETA2 . RES ' , STATUS= 

'NEW' ) 

OPEN 

(13,FILE= 'THETA3.RES' , STATUS= 

' NEW  *  ) 

OPEN 

(14,FILE= 'THETA4.RES' , STATUS= 

'NEW  ) 

OPEN 

(21, FILE= ' DELTA1 . RES ' , STATUS= 

' NEW ' ) 

OPEN 

( 22 , FILE= ' DELTA2 . RES ' , STATUS= 

'NEW' ) 

SAT  =0.4 

SATP=  SAT 

SATM=-SAT 

PI   =4.0*ATAN(1.0 
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10 


20 


30 


15 


WRITE 
READ 
GO  TO 

WRITE 
READ 


(* 
(* 


(10,20,30),  IVAR 
( 


( 


INCR=IU 


WRITE 

READ 

WRITE 

READ 

GO  TO 

WRITE 

READ 


(* 
(* 
(* 
(* 

15 

(* 
(* 


INCR=IZG 


WRITE 

READ 

WRITE 

READ 

GO  TO 

WRITE 

READ 


(* 
(* 
(* 
(* 
15 

(* 
(* 


INCR=ITV 


WRITE 
READ 
WRITE 
READ 


(* 

(* 
(* 
(* 

1  =  1 


1001) 
*) 


IVAR 


1002) 
*) 

1003) 

*) 

1006) 

*) 

1004) 
*) 

1005) 

*) 

1006) 

*) 

1007) 
*) 

1003) 

*) 
1005) 

*) 


UMIN,UMAX,IU 


ZG 


TV 


ZGMIN,ZGMAX,IZG 


U 


TV 


TVMIN,TVMAX, ITV 


ZG 


U 


DO  1 

IF 

IF 

IF 

DV   = 

A11V= 

A12V= 


.  INCR 
(IVAR.EQ.l) 
(IVAR.EQ.2) 
(IVAR.EQ.3) 


U  =UMIN  +(UMAX  -UMIN  ) * ( 1-1 ) / ( INCR-1 ) 
ZG=ZGMIN+(ZGMAX-ZGMIN)*(I-1)/(INCR-1) 
TV=TVMIN+ ( TVMAX-TVMIN ) * ( I - 1 ) / ( INCR- 1 ) 

MASS-ZWDOT)*(IY-MQDOT)-ZQDOT*MWDOT 

( IY-MQDOT)*ZW+ZQDOT*MW)/DV 

( I Y-MQDOT ) * ( ZQ+MASS ) +ZQDOT*MQ ) /DV 


A13V=- ( ZG-ZB ) * (MASS*XG+ZQDOT) *WEIGHT/DV 
A2 lV=(MWDOT*ZW+( MASS- ZWDOT )*MW)/DV 
A22V=(MWDOT*( ZQ+MASS )+ (MASS- ZWDOT ) *MQ) /DV 
A23V=- ( ZG-ZB ) * (MASS-ZWDOT) *WEIGHT/DV 
B11V=( (IY-MQDOT)*ZDS+ZQDOT*MDS)/DV 
B12V=( (IY-MQDOT)*ZDB+ZQDOT*MDB)/DV 
B21V=(MWDOT*ZDS+ (MASS- ZWDOT )*MDS)/DV 
B22V=(MWDOT*ZDB+ (MASS- ZWDOT )*MDB)/DV 
B1V  =B11V-B12V 
B2V  =B21V-B22V 


OMEGAV= ( 1 0 . 0  *U ) / ( TV*L ) 

AD1V=1.75*0MEGAV 

AD2V=2 . 15*OMEGAV**2 

AD3V=OMEGAV**3 

A2=B1V*U*U 

A3=B2V*U*U 
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D1=-AD1V- ( A11V+A22V) *U 

B1=-B2V*U*U 

B2=(B1V*A22V-B2V*A12V)*U**3 

B3=(B2V*A11V-B1V*A21V)*U**3 

D2=AD2V+A2  3V+(A12V*A21V-A11V*A22V)*U**2 

C1=(B2V*A11V-B1V*A21V)*U**3 

C2=(A2  3V*B1V-A13V*B2V)*U**2 

D3=AD3V+(A13V*A21V-A11V*A23V)*U 

K2V=(A3*B1*D3+C1*B3*D1-D2*C1*A3) 

K2V=K2V/(A3*B1*C2+C1*B3*A2-C1*A3*B2) 

K1V=(D3-C2*K2V)/C1 

K3V=(D1-A2*K2V)/A3 

IF  (IVAR.EQ.l)  OUT=U 

IF  (IVAR.EQ.2)  OUT=ZG 

IF  (IVAR.EQ.3)  OUT=TV 

D3P=(A13V*A21V-A11V*A2  3V)*U 

XAAA=CBRT(-D3P) 

TVCR=( 10.*U)/(XAAA*L) 

IF  (TV.LT.TVCR)  GO  TO  1 


CALL  S0LSET(INUM,THS0LS,K1V,C1,AD3V,SSTH) 

ICHECK=0 

DO  2  111=1, INUM 

THCH=2 . 0* (THSOLS-0 . 5*PI ) 
CHECK=SIN(THCH)*(D3-AD3V)/C1 
IF  ( ABS( CHECK ) .GT.SATP)  GO  TO  2 
WRITE  (13,*)  OUT,  THCH*180.0/PI 
WRITE  (14,*)  OUT,-THCH*180.0/PI 
WRITE  (21,*)  OUT,  ABS(CHECK)*180.0/PI 
ICHECK=1 

CONTINUE 

IF  (ICHECK.EQ.O)  GO  TO  3 

GO  TO  1 


1001 


1002 
1003 
1004 
1005 


STHETA=SATP*C1/(D3-AD3V) 
SSTH=ASIN(STHETA) 
THETA1=  SSTH*180.0/PI 
THETA2=-SSTH*180.0/PI 
WRITE  (11,*)  OUT,THETAl 


WRITE 

( 

12,*)  OUT,THETA2 

WRITE 

(22,*)  OUT,  SATP*180.0/PI 

CONTINUE 

STOP 

FORMAT 

( 

ENTER  1  :  U   VARIATION',/, 

& 

2  :  ZG  VARIATION' ,/, 

& 

3  :  TV  VARIATION' ) 

FORMAT 

( 

ENTER  MIN,  MAX,  AND  INCREMENTS 

FORMAT 

( 

ENTER  ZG' ) 

FORMAT 

( 

ENTER  MIN,  MAX,  AND  INCREMENTS 

FORMAT 

( 

ENTER  U' ) 

IN  U'  ) 

IN  ZG' ) 
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1006  FORMAT  ('  ENTER  TV) 

1007  FORMAT  ('  ENTER  MIN,  MAX,  AND  INCREMENTS  IN  TV) 

END 
C 

FUNCTION  CBRT(A) 

IF  (A.GT.0.0)  CBRT=    A  **(l./3.) 
IF  (A.LE.0.0)  CBRT=-(-A)**(l./3. ) 
RETURN 
END 
C 

SUBROUTINE  SOLSET( L, ANS , K1V, CI , AD3V, SSTH) 
REAL  K1V 

DIMENSION  VF(1,2) 
C 

PI=4.0*ATAN(1.0) 
C 

C      FIND  FIRST  ESTIMATE  OF  THE  SOLUTIONS 
L=0 

VMIN=  0.0 
VMAX=+90.0 
IV=100 

VA=VMIN*PI/180.0 
VAO=VA 

V0=THETEQ(1,VA,K1V,C1,AD3V) 
DO  10  1=2, IV 

VA=VMIN+(VMAX-VMIN) * ( 1-1 ) / ( IV- 1 ) 

VA=VA*PI/180.0 

VAN=VA 

VN=THETEQ(1,VA,K1V,C1,AD3V) 

VP=VO*VN 

IF  (VP.GE.0.0)  GO  TO  11 

L=L+1 

VF(L, l)=VAO 

VF(L,2)=VAN 

GO  TO  12 

1 1  VO=VN 
VAO=VAN 

10  CONTINUE 
C 
C      EXACT  COMPUTATION  OF  SOLUTIONS  VIA  NEWTON'S  METHOD 

12  E=l.E-5 
IEND=500 

DO  20  J=1,L 

X=(VF(J, 1)+VF( J, 2) )/2.0 
F=THETEQ( 1 , X, K1V, CI , AD3V) 
FDER=THETEQ(2,X,K1V,C1,AD3V) 
DO  30  K=1,IEND 

IF  (FDER.EQ.0.0)  STOP  1001 

DX=F/FDER 

X1=X-DX 

F=THETEQ(1,X1,K1V,C1,AD3V) 
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FDER=THETEQ (2,X1,K1V,C1, AD3V ) 
IF  (F.EQ.O. )  GO  TO  35 
A=ABS(X1-X) 
IF  (A-E)  35,35,40 
40      X=X1 
30    CONTINUE 
GO  TO  20 
35    ANS=X1 
20  CONTINUE 
RETURN 
END 

FUNCTION  THETEQ(K,THETA,K1V,C1,AD3V) 

REAL  K1V 

GO  TO  (10,20)  ,  K 
10  THETEQ=K1V*C1*THETA+(AD3V-K1V*C1)*C0S(THETA) 

GO  TO  50 
20  THETEQ=K1V*C1- (AD3V-K1V*C1 ) *SIN(THETA) 
50  RETURN 

END 
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